sysId

FRC3636 · Nov 14, 2024 · 9caa6d1 · 9caa6d1
1 parent 6b220d2
commit 9caa6d1
Show file tree

Hide file tree

Showing 3 changed files with 81 additions and 23 deletions.
diff --git a/src/main/java/com/frcteam3636/frc2024/Robot.kt b/src/main/java/com/frcteam3636/frc2024/Robot.kt
@@ -1,6 +1,7 @@
 package com.frcteam3636.frc2024
 
 import BuildConstants
+import com.ctre.phoenix6.SignalLogger
 import com.ctre.phoenix6.StatusSignal
 import com.frcteam3636.frc2024.subsystems.arm.Arm
 import com.frcteam3636.frc2024.subsystems.drivetrain.Drivetrain
@@ -16,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler
 import edu.wpi.first.wpilibj2.command.Commands
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController
 import edu.wpi.first.wpilibj2.command.button.JoystickButton
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
 import org.littletonrobotics.junction.LogFileUtil
 import org.littletonrobotics.junction.LoggedRobot
 import org.littletonrobotics.junction.Logger
@@ -130,36 +132,55 @@ object Robot : LoggedRobot() {
             Drivetrain.zeroGyro()
         }).ignoringDisable(true))
 
-        //Intake
-        controller.rightBumper()
-            .debounce(0.150)
-            .whileTrue(
-                Intake.intake()
-            )
+//        //Intake
+//        controller.rightBumper()
+//            .debounce(0.150)
+//            .whileTrue(
+//                Intake.intake()
+//            )
+//
+//        //Outtake
+//        controller.leftBumper()
+//            .whileTrue(
+//                Commands.parallel(
+//                    Intake.outtake(),
+//                )
+//            )
 
-        //Outtake
+        //SysId
         controller.leftBumper()
-            .whileTrue(
-                Commands.parallel(
-                    Intake.outtake(),
-                )
-            )
+            .onTrue(Commands.runOnce(SignalLogger::start))
+
+        controller.rightBumper()
+            .onTrue(Commands.runOnce(SignalLogger::stop))
+
+        controller.y()
+            .whileTrue(Arm.sysIdQuasistatic(SysIdRoutine.Direction.kForward))
 
-        //Arm positions
         controller.a()
-            .onTrue(
-                Arm.moveToPosition(Arm.Position.Stowed)
-            )
+            .whileTrue(Arm.sysIdQuasistatic(SysIdRoutine.Direction.kReverse))
+
+        controller.b()
+            .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kForward))
 
         controller.x()
-            .onTrue(
-                Arm.moveToPosition(Arm.Position.PickUp)
-            )
+            .whileTrue(Arm.sysIdDynamic(SysIdRoutine.Direction.kReverse))
 
-        controller.y()
-            .onTrue(
-                Arm.moveToPosition(Arm.Position.Lower)
-            )
+        //Arm positions
+//        controller.a()
+//            .onTrue(
+//                Arm.moveToPosition(Arm.Position.Stowed)
+//            )
+//
+//        controller.x()
+//            .onTrue(
+//                Arm.moveToPosition(Arm.Position.PickUp)
+//            )
+//
+//        controller.y()
+//            .onTrue(
+//                Arm.moveToPosition(Arm.Position.Lower)
+//            )
     }
 
     /** Add data to the driver station dashboard. */

diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/arm/Arm.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/arm/Arm.kt
@@ -1,16 +1,37 @@
 package com.frcteam3636.frc2024.subsystems.arm
 
+import com.ctre.phoenix6.SignalLogger
 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.Volts
+import edu.wpi.first.units.Voltage
 import edu.wpi.first.wpilibj2.command.Subsystem
+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
 import org.littletonrobotics.junction.Logger
 
+
 object Arm : Subsystem {
     var io = ArmIOReal()
 
     var inputs = ArmIO.ArmInputs()
 
+    var sysID = SysIdRoutine(
+        SysIdRoutine.Config(
+            null,
+            Volts.of(4.0),
+            null,
+            { state -> SignalLogger.writeString("state", state.toString()) }
+        ),
+        Mechanism(this::voltageDrive, null, this)
+    )
+
+    private fun voltageDrive(measure: Measure<Voltage>) {
+        io.setVoltage(measure)
+    }
+
     override fun periodic() {
         io.updateInputs(inputs)
 
@@ -24,6 +45,11 @@ object Arm : Subsystem {
             io.setPosition(inputs.position)
         })
 
+    fun sysIdQuasistatic(direction: Direction) =
+        sysID.quasistatic(direction)
+
+    fun sysIdDynamic(direction: Direction) =
+        sysID.dynamic(direction)
 
     enum class Position(val angle: Measure<Angle>) {
         Stowed(Degrees.of(135.0)),

diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt
@@ -1,6 +1,7 @@
 package com.frcteam3636.frc2024.subsystems.arm
 import com.ctre.phoenix6.configs.TalonFXConfiguration
 import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC
+import com.ctre.phoenix6.controls.VoltageOut
 import com.ctre.phoenix6.signals.GravityTypeValue
 import com.ctre.phoenix6.signals.InvertedValue
 import com.ctre.phoenix6.signals.NeutralModeValue
@@ -13,6 +14,7 @@ import com.frcteam3636.frc2024.utils.math.pidGains
 import edu.wpi.first.units.Angle
 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.DutyCycleEncoder
 import org.littletonrobotics.junction.LogTable
@@ -68,6 +70,8 @@ interface ArmIO{
         fun updateInputs(inputs: ArmInputs)
 
         fun setPosition(position: Measure<Angle>)
+
+    fun setVoltage(volts: Measure<Voltage>)
     }
 
     class ArmIOReal: ArmIO{
@@ -107,6 +111,13 @@ interface ArmIO{
             leftMotor.setControl(leftControl)
 
         }
+
+        override fun setVoltage(volts: Measure<Voltage>) {
+            val control = VoltageOut(volts.`in`(Volts))
+            leftMotor.setControl(control)
+            rightMotor.setControl(control)
+        }
+
         init {
             val config = TalonFXConfiguration().apply {
                 MotorOutput.apply {