feat: guessing and checking

FRC3636 · Dec 12, 2024 · 9edc483 · 9edc483
1 parent b326f2b
commit 9edc483
Show file tree

Hide file tree

Showing 7 changed files with 113 additions and 59 deletions.
diff --git a/src/main/kotlin/com/frcteam3636/frc2024/CAN.kt b/src/main/kotlin/com/frcteam3636/frc2024/CAN.kt
@@ -34,8 +34,8 @@ enum class CTREDeviceId(val num: Int, val bus: String) {
     BackLeftDrivingMotor(2, "*"),
     BackRightDrivingMotor(3, "*"),
     FrontRightDrivingMotor(4, "*"),
-    RightArmMotor(10, "*"),
-    LeftArmMotor(11, "*"),
+    RightArmMotor(11, "*"),
+    LeftArmMotor(10, "*"),
     PigeonGyro(20, "*"),
 }
 

diff --git a/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt b/src/main/kotlin/com/frcteam3636/frc2024/Robot.kt
@@ -74,11 +74,6 @@ object Robot : PatchedLoggedRobot() {
         configureAutos()
         configureBindings()
         configureDashboard()
-
-        Intake.register()
-
-        //configure bindings
-        configureBindings()
     }
 
     /** Start logging or pull replay logs from a file */
@@ -155,16 +150,16 @@ object Robot : PatchedLoggedRobot() {
 
     /** Configure which commands each joystick button triggers. */
     private fun configureBindings() {
-        Drivetrain.defaultCommand = Drivetrain.driveWithJoysticks(joystickLeft, joystickRight)
-        Indexer.defaultCommand = Indexer.autoRun()
+//         Drivetrain.defaultCommand = Drivetrain.driveWithJoysticks(joystickLeft, joystickRight)
+//        Indexer.defaultCommand = Indexer.autoRun()
 
         // (The button with the yellow tape on it)
         JoystickButton(joystickLeft, 8).onTrue(Commands.runOnce({
             println("Zeroing gyro.")
             Drivetrain.zeroGyro()
         }).ignoringDisable(true))
 
-//        //Intake
+        //Intake
 //        controller.a()
 //            .debounce(0.150)
 //            .whileTrue(
@@ -178,6 +173,17 @@ object Robot : PatchedLoggedRobot() {
 //            )
 //
 //        controller.b()
+//            .debounce(0.15)
+//            .whileTrue(
+//                Indexer.indexBalloon()
+//            )
+//        controller.y()
+//            .debounce(0.15)
+//            .whileTrue(
+//                Indexer.outtakeBalloon()
+//            )
+//
+//        controller.b()
 //            .debounce(0.150)
 //            .whileTrue(
 //                Indexer.outtakeBalloon()
@@ -205,18 +211,26 @@ object Robot : PatchedLoggedRobot() {
             .onTrue(Commands.runOnce(SignalLogger::stop))
 
         controller.y()
-            .whileTrue(Arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward))
+            .whileTrue(
+                Commands.sequence(
+                    Commands.print("Starting quasistatic"),
+                    Arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward)
+                )
+                    .until(Arm.inSysIdUpperRange.negate())
+            )
+
 
         controller.a()
             .whileTrue(Arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse))
 
-        controller.b()
-            .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kForward))
-
-        controller.x()
+        controller.rightBumper()
+            .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kForward)
+//            .until(Arm.inSysIdUpperRange.negate()))
+            )
+        controller.leftBumper()
             .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kReverse))
 
-        //Arm positions
+        //Arm positions1
 //        controller.a()
 //            .onTrue(
 //                Arm.moveToPosition(Arm.Position.Stowed)

diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt
@@ -8,13 +8,15 @@ import com.frcteam3636.frc2024.subsystems.intake.Intake.intakeAngleLigament
 import edu.wpi.first.units.Angle
 import edu.wpi.first.units.Measure
 import edu.wpi.first.units.Units.Degrees
+import edu.wpi.first.units.Units.Radians
 import edu.wpi.first.units.Units.Volts
 import edu.wpi.first.wpilibj.Timer
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d
 import edu.wpi.first.wpilibj.util.Color
 import edu.wpi.first.wpilibj.util.Color8Bit
 import edu.wpi.first.wpilibj2.command.Subsystem
+import edu.wpi.first.wpilibj2.command.button.Trigger
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism
@@ -23,7 +25,7 @@ import kotlin.math.cos
 import kotlin.math.sin
 
 
-private const val SECONDS_BETWEEN_ARM_UPDATES = 2.0
+private const val SECONDS_BETWEEN_ARM_UPDATES = 1.0
 
 object Arm : Subsystem {
     private var io: ArmIO = when (Robot.model) {
@@ -53,6 +55,13 @@ object Arm : Subsystem {
         Mechanism(io::setVoltage, null, this)
     )
 
+    var inSysIdUpperRange = Trigger {
+        val lowerLimit = Radians.of(3.167)
+        inputs.position < lowerLimit
+                && inputs.leftPosition < lowerLimit
+//                && inputs.rightPosition < lowerLimit
+    }
+
     private var timer = Timer().apply {
         start()
     }
@@ -64,8 +73,9 @@ object Arm : Subsystem {
         armWristAngleLigament.angle = 90.0 - inputs.position.`in`(Degrees)
 
         if (timer.advanceIfElapsed(SECONDS_BETWEEN_ARM_UPDATES) && inputs.absoluteEncoderConnected){
-                io.updatePosition(inputs.absoluteEncoderPosition)
+                io.updatePosition(inputs.position)
         }
+
         Logger.recordOutput("/Arm/Mechanism", mechanism)
 
     }
@@ -84,8 +94,8 @@ object Arm : Subsystem {
         sysID.dynamic(direction)!!
 
     enum class Position(val angle: Measure<Angle>) {
-        Stowed(Degrees.of(135.0)),
-        PickUp(Degrees.of(30.0)),
-        Lower(Degrees.of(-15.0))
+        Stowed(Radians.of(0.0)),
+        PickUp(Radians.of(-0.6)),
+        Lower(Radians.of(0.0))
     }
 }
diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt
@@ -17,9 +17,7 @@ import edu.wpi.first.units.Voltage
 import edu.wpi.first.wpilibj.DigitalInput
 import edu.wpi.first.wpilibj.DutyCycleEncoder
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim
-import org.littletonrobotics.junction.LogTable
 import org.littletonrobotics.junction.Logger
-import org.littletonrobotics.junction.inputs.LoggableInputs
 import org.team9432.annotation.Logged
 
 @Logged
@@ -28,8 +26,6 @@ open class ArmInputs {
     var leftPosition = Radians.zero()!!
     var position = Radians.zero()!!
 
-    var absoluteEncoderPosition = Radians.zero()!!
-
     var rightCurrent = Volts.zero()!!
     var leftCurrent = Volts.zero()!!
 
@@ -60,11 +56,12 @@ class ArmIOReal: ArmIO {
     private val absoluteEncoder = DutyCycleEncoder(DigitalInput(0))
 
     override fun updateInputs(inputs: ArmInputs) {
-        inputs.position = Rotations.of(absoluteEncoder.absolutePosition)
+        val unoffsetPosition = Rotations.of(-absoluteEncoder.get() * CHAIN_GEAR_RATIO)
+        inputs.position = unoffsetPosition + ZERO_OFFSET
+        Logger.recordOutput("/Arm/Required Offset", unoffsetPosition.negate())
+//        inputs.position = Rotations.of(-absoluteEncoder.absolutePosition) + ZERO_OFFSET
         inputs.absoluteEncoderConnected = absoluteEncoder.isConnected
 
-        inputs.absoluteEncoderPosition = Rotations.of(absoluteEncoder.absolutePosition)
-
         inputs.leftPosition = Rotations.of(leftMotor.position.value)
         inputs.leftVelocity = RotationsPerSecond.of(leftMotor.velocity.value)
 
@@ -81,22 +78,24 @@ class ArmIOReal: ArmIO {
     }
 
     override fun pivotToPosition(position: Measure<Angle>) {
-        Logger.recordOutput("Shooter/Pivot/Position Setpoint", position)
+        val resolvedPosition = position
+        Logger.recordOutput("Shooter/Pivot/Position Setpoint", resolvedPosition)
 
         val control = MotionMagicTorqueCurrentFOC(0.0).apply {
             Slot = 0
-            Position = position.`in`(Rotations)
+            Position = resolvedPosition.`in`(Rotations)
         }
         leftMotor.setControl(control)
-        rightMotor.setControl(control)
+//        rightMotor.setControl(control)
 
     }
 
     override fun setVoltage(volts: Measure<Voltage>) {
         assert(volts in Volts.of(-12.0)..Volts.of(12.0))
+        Logger.recordOutput("/Arm/Voltage", volts)
         val control = VoltageOut(volts.`in`(Volts))
-        leftMotor.setControl(control)
-        rightMotor.setControl(control)
+        leftMotor.setControl(control) // TODO: fix gearbox
+//        rightMotor.setControl(control)
     }
 
     init {
@@ -122,6 +121,10 @@ class ArmIOReal: ArmIO {
                 MotionMagicAcceleration = PROFILE_ACCELERATION
                 MotionMagicJerk = PROFILE_JERK
             }
+
+            ClosedLoopGeneral.apply {
+                ContinuousWrap = true
+            }
         }
 
         config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive
@@ -135,13 +138,24 @@ class ArmIOReal: ArmIO {
 
 
     internal companion object Constants {
-        private const val GEAR_RATIO = 0.0
-        val PID_GAINS = PIDGains(120.0, 0.0, 100.0) //placeholders
-        val FF_GAINS = MotorFFGains(7.8, 0.0, 0.0) //placeholders
-        private const val GRAVITY_GAIN = 0.0
-        private const val PROFILE_ACCELERATION = 0.0
-        private const val PROFILE_JERK = 0.0
-        private const val PROFILE_VELOCITY = 0.0
+//        private const val GEAR_RATIO = 125.0
+//        val PID_GAINS = PIDGains(60.88, 0.0, 0.40035) //placeholders
+//        val FF_GAINS = MotorFFGains(0.1448, 0.11929, 0.001579) //placeholders
+//        private const val GRAVITY_GAIN = 0.0095692
+//        private const val PROFILE_ACCELERATION = 1.0
+//        private const val PROFILE_JERK = 1.0
+//        private const val PROFILE_VELOCITY = 1.0
+
+        private const val CHAIN_GEAR_RATIO = 1.0 / 3.0
+        private const val GEAR_RATIO = 125.0 / CHAIN_GEAR_RATIO
+        val PID_GAINS = PIDGains(64.857, 0.0, 20.319) //placeholders
+        val FF_GAINS = MotorFFGains(0.33157, 14.214, 0.15033) //placeholders
+        private const val GRAVITY_GAIN = 0.28233
+        private const val PROFILE_ACCELERATION = 1.0
+        private const val PROFILE_JERK = 1.0
+        private const val PROFILE_VELOCITY = 1.0
+
+        val ZERO_OFFSET = Radians.of(1.01)
     }
 
 }

diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt
@@ -4,13 +4,14 @@ import com.frcteam3636.frc2024.Robot
 import com.frcteam3636.frc2024.subsystems.intake.Intake
 import edu.wpi.first.units.Units.Degrees
 import edu.wpi.first.wpilibj.DriverStation
+import edu.wpi.first.wpilibj.Timer
 import edu.wpi.first.wpilibj.util.Color
 import edu.wpi.first.wpilibj.util.Color8Bit
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.Subsystem
 import org.littletonrobotics.junction.Logger
 
-object Indexer: Subsystem {
+object Indexer : Subsystem {
     private var io: IndexerIO = when (Robot.model) {
         Robot.Model.SIMULATION -> IndexerIOSim()
         Robot.Model.COMPETITION -> IndexerIOReal()
@@ -36,34 +37,48 @@ object Indexer: Subsystem {
      * Does not run if no balloon.
      * Reverses if balloon is wrong alliance.
      */
-    fun autoRun(): Command =
-        runEnd(
+    fun autoRun(): Command {
+        var previousState = BalloonState.None;
+        var timer = Timer()
+        timer.start()
+        return runEnd(
             {
-                DriverStation.getAlliance().map {
-                    if (
-                        (inputs.balloonState == BalloonState.Blue && it == DriverStation.Alliance.Blue)
-                        || (inputs.balloonState == BalloonState.Red && it == DriverStation.Alliance.Red)
-                    ) {
-                        io.setSpinSpeed(0.5)
-                    } else if (inputs.balloonState == BalloonState.None) {
-                        io.setSpinSpeed(0.0)
-                    } else {
-                        io.setSpinSpeed(-0.5)
+                if (previousState != inputs.balloonState) {
+                    timer.reset()
+                }
+
+                if (timer.hasElapsed(0.15)) {
+                    DriverStation.getAlliance().map {
+                        if (
+                            (inputs.balloonState == BalloonState.Blue && it == DriverStation.Alliance.Blue)
+                            || (inputs.balloonState == BalloonState.Red && it == DriverStation.Alliance.Red)
+
+                        ) {
+
+                            io.setSpinSpeed(0.1)
+                        } else if (inputs.balloonState == BalloonState.None) {
+                            io.setSpinSpeed(0.0)
+                        } else {
+                            io.setSpinSpeed(-0.1)
+                        }
                     }
                 }
+
+                previousState = inputs.balloonState;
             },
             {
                 io.setSpinSpeed(0.0)
             }
         )
+    }
 
     fun indexBalloon(): Command = runEnd(
-        {io.setSpinSpeed(0.5)},
-        {io.setSpinSpeed(0.0)}
+        { io.setSpinSpeed(0.05) },
+        { io.setSpinSpeed(0.0) }
     )
 
     fun outtakeBalloon(): Command = runEnd(
-        {io.setSpinSpeed(-0.5)},
-        {io.setSpinSpeed(0.0)}
+        { io.setSpinSpeed(-0.05) },
+        { io.setSpinSpeed(0.0) }
     )
 }
diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt
@@ -49,6 +49,7 @@ class IndexerIOReal : IndexerIO{
             CANSparkLowLevel.MotorType.kBrushless
         )
 
+
     override fun updateInputs(inputs: IndexerInputs) {
         inputs.indexerVelocity = Rotations.per(Minute).of(indexerMotor.encoder.velocity)
         inputs.indexerCurrent = Amps.of(indexerMotor.outputCurrent)

diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/intake/Intake.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/intake/Intake.kt
@@ -43,7 +43,7 @@ object Intake: Subsystem {
     fun outtake(): Command =
         startEnd(
             {
-                io.setSpeed(-0.5)
+                io.setSpeed(-0.1)
             },
             {
                 io.setSpeed(0.0)
@@ -53,7 +53,7 @@ object Intake: Subsystem {
     fun intake(): Command =
         startEnd(
             {
-                io.setSpeed(0.7)
+                io.setSpeed(0.1)
             },
             {
                 io.setSpeed(0.0)