Pushed Code

Team2537 · Mar 22, 2024 · 0b2eb95 · 0b2eb95
1 parent 35035e0
commit 0b2eb95
Show file tree

Hide file tree

Showing 7 changed files with 61 additions and 30 deletions.
diff --git a/src/main/java/frc/robot/Constants.kt b/src/main/java/frc/robot/Constants.kt
@@ -99,10 +99,10 @@ object Constants {
         const val slope = -0.33
 
         val distanceMap: HashMap<Double, Double> = hashMapOf(
-            Pair(0.0 + 48.5, 81.5),
-            Pair(25.5 + 48.5, 69.1),
-            Pair(69.0 + 48.5, 61.7),
-            Pair(94.0 + 48.5, 57.0),
+            Pair(0.0 + 54.17, 81.5),
+            Pair(78.75, 72.75),
+            Pair(104.33, 65.78),
+            Pair(187.4, 60.5)
         )
     }
 

diff --git a/src/main/java/frc/robot/RobotContainer.kt b/src/main/java/frc/robot/RobotContainer.kt
@@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.Commands
 import edu.wpi.first.wpilibj2.command.InstantCommand
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup
 import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController
 import edu.wpi.first.wpilibj2.command.button.Trigger
 import frc.robot.commands.Autos
@@ -83,10 +84,10 @@ object RobotContainer {
         { controller.leftTriggerAxis }
     )
 
-    val trackSpeakerCommand = ParallelCommandGroup(
-        RotateTowardsTargetCommand(LimelightSubsystem.odometryLimelight),
-        QuickPivotCommand(0.0, false, true)
-    )
+//    val trackSpeakerCommand = ParallelCommandGroup(
+//        RotateTowardsTargetCommand(LimelightSubsystem.odometryLimelight),
+//        QuickPivotCommand(0.0, false, true)
+//    )
 
     val intakePivot: QuickPivotCommand = QuickPivotCommand(Constants.PivotConstants.INTAKE_POSITION, false, false)
     val subwooferPivot: QuickPivotCommand = QuickPivotCommand(Constants.PivotConstants.SUBWOOFER_POSITION, false, false)
@@ -155,23 +156,29 @@ object RobotContainer {
             )
         )
 
-        controller.rightBumper().onTrue(
-            Commands.runOnce({
-                if (teleopDrive.isScheduled) {
-                    teleopDrive.cancel()
-                    trackSpeakerCommand.schedule()
-                } else {
-                    trackSpeakerCommand.cancel()
-                    teleopDrive.schedule()
-                }
-            })
-        )
+//        controller.rightBumper().onTrue(
+//            Commands.runOnce({
+//                if (teleopDrive.isScheduled) {
+//                    teleopDrive.cancel()
+//                    trackSpeakerCommand.schedule()
+//                } else {
+//                    trackSpeakerCommand.cancel()
+//                    teleopDrive.schedule()
+//                }
+//            })
+//        )
 
         controller.leftStick().onTrue(InstantCommand(SwerveSubsystem::zeroGyro))
         controller.povUp().onTrue(ampPivot)
         controller.povRight().onTrue(intakePivot)
         controller.povDown().onTrue(subwooferPivot)
-        controller.povLeft().onTrue(autoAim) // TODO: Implement auto-aiming
+        controller.povLeft().onTrue(
+            autoAim
+        ) // TODO: Implement auto-aiming
+        controller.rightBumper().toggleOnTrue(
+            RotateTowardsTargetCommand(LimelightSubsystem.odometryLimelight)
+        )
+
         controller.y().onTrue(HomePivotCommand()) // TODO: Implement homing launcher
         controller.b().toggleOnTrue(manualPivot)
         controller.x().onTrue(manualClimb)

diff --git a/src/main/java/frc/robot/commands/Autos.kt b/src/main/java/frc/robot/commands/Autos.kt
@@ -57,7 +57,7 @@ object Autos {
         ))
         NamedCommands.registerCommand("Pull Note", IntakeNoteCommand())
         NamedCommands.registerCommand("Intake Note", ToggleIntakeCommand())
-        NamedCommands.registerCommand("Set Down", SetKnownPosition(90.1))
+        NamedCommands.registerCommand("Set Down", SetKnownPosition(91.28))
         NamedCommands.registerCommand("Intake Sequence",
             ParallelDeadlineGroup(
                 IntakeNoteCommand(),

diff --git a/src/main/java/frc/robot/commands/launcher/LaunchCommand.kt b/src/main/java/frc/robot/commands/launcher/LaunchCommand.kt
@@ -33,6 +33,7 @@ class LaunchCommand(
     }
 
     override fun initialize() {
+        launcherSubsystem.setFlywheelBrake(false)
         timer.restart()
         println("Starting Launch")
     }
@@ -42,7 +43,7 @@ class LaunchCommand(
             launcherSubsystem.setFlywheelSpeeds(speed.asDouble / 4)
             minVelocity = 1200.0
         } else {
-            launcherSubsystem.setFlywheelVelocity(6000.0.rpm)
+            launcherSubsystem.setFlywheelVelocity(5800.0.rpm)
             minVelocity = 5500.0
 
         }
@@ -66,6 +67,7 @@ class LaunchCommand(
         launcherSubsystem.stopFlywheels()
         launcherSubsystem.rollerMotor.setIdleMode(CANSparkBase.IdleMode.kBrake)
         launcherSubsystem.stopRoller()
+        launcherSubsystem.setFlywheelBrake(true)
         println("Launch Ending $interrupted")
     }
 }
diff --git a/src/main/java/frc/robot/commands/pivot/ManualPivotCommand.kt b/src/main/java/frc/robot/commands/pivot/ManualPivotCommand.kt
@@ -27,6 +27,6 @@ class ManualPivotCommand(voltage: DoubleSupplier) : Command() {
     }
 
     override fun end(interrupted: Boolean) {
-        QuickPivotCommand(pivotSubsystem.getRelativePosition(), false, false).schedule()
+//        QuickPivotCommand(pivotSubsystem.getRelativePosition(), false, false).schedule()
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/LauncherSubsystem.kt b/src/main/java/frc/robot/subsystems/LauncherSubsystem.kt
@@ -4,7 +4,6 @@ import edu.wpi.first.units.Measure
 import edu.wpi.first.units.Units
 import edu.wpi.first.units.Voltage
 import edu.wpi.first.wpilibj.DigitalInput
-import edu.wpi.first.wpilibj.Timer
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard
 import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog
 import edu.wpi.first.wpilibj2.command.Command
@@ -16,10 +15,7 @@ import frc.robot.Constants
 import frc.robot.Robot
 import lib.math.units.RotationVelocity
 import lib.math.units.into
-import lib.math.units.rpm
-import lib.math.units.velocity
-import lib.near
-import lib.zoneTrigger
+import kotlin.math.min
 
 object LauncherSubsystem : SubsystemBase() {
     // TODO: Get the actual IDs
@@ -120,6 +116,16 @@ object LauncherSubsystem : SubsystemBase() {
         bottomFlywheels.encoder.setPosition(0.0)
     }
 
+    fun setFlywheelBrake(brake: Boolean){
+        if(brake){
+            topFlywheels.setIdleMode(CANSparkBase.IdleMode.kBrake)
+            bottomFlywheels.setIdleMode(CANSparkBase.IdleMode.kBrake)
+        } else {
+            topFlywheels.setIdleMode(CANSparkBase.IdleMode.kCoast)
+            bottomFlywheels.setIdleMode(CANSparkBase.IdleMode.kCoast)
+        }
+    }
+
     fun setFlywheelSpeeds(rawSpeed: Double) {
         topFlywheels.set(rawSpeed)
         bottomFlywheels.set(rawSpeed)
@@ -130,8 +136,12 @@ object LauncherSubsystem : SubsystemBase() {
     }
 
     fun setFlywheelVelocity(velocity: RotationVelocity){
-        topFlywheels.setVoltage(topFlywheelFeedforward.calculate(velocity into Units.RotationsPerSecond));
-        bottomFlywheels.setVoltage(bottomFlywheelFeedforward.calculate(velocity into Units.RotationsPerSecond))
+        val currentV = min(topFlywheels.encoder.velocity, bottomFlywheels.encoder.velocity)
+        val error = velocity.into(Units.RPM) - currentV
+        val extraVoltage = error * 0.002
+
+        topFlywheels.setVoltage(topFlywheelFeedforward.calculate(velocity into Units.RotationsPerSecond) + extraVoltage);
+        bottomFlywheels.setVoltage(bottomFlywheelFeedforward.calculate(velocity into Units.RotationsPerSecond) + extraVoltage)
     }
 
     fun setFlywheelVoltage(voltage: Double){

diff --git a/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt b/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt
@@ -1,7 +1,9 @@
 package frc.robot.subsystems
 
+import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj2.command.SubsystemBase
 import lib.vision.Limelight
+import java.sql.Driver
 
 /**
  * The subsystem that controls the limelight.
@@ -13,4 +15,14 @@ object LimelightSubsystem : SubsystemBase() {
     init {
         odometryLimelight.setTargetTag(7)
     }
+
+    override fun periodic() {
+        if(DriverStation.getAlliance().isPresent){
+            if(DriverStation.getAlliance().get() == DriverStation.Alliance.Red){
+                odometryLimelight.setTargetTag(4)
+            } else {
+                odometryLimelight.setTargetTag(7)
+            }
+        }
+    }
 }