BOB wrist tuning + wvrox code

src/main/kotlin/com/team4099/robot2023/RobotContainer.kt

@@ -13,12 +13,14 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim
 import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO
 import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2
 import com.team4099.robot2023.subsystems.elevator.Elevator
+import com.team4099.robot2023.subsystems.elevator.ElevatorIO
 import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO
 import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim
 import com.team4099.robot2023.subsystems.feeder.Feeder
 import com.team4099.robot2023.subsystems.feeder.FeederIONeo
 import com.team4099.robot2023.subsystems.feeder.FeederIOSim
 import com.team4099.robot2023.subsystems.flywheel.Flywheel
+import com.team4099.robot2023.subsystems.flywheel.FlywheelIO
 import com.team4099.robot2023.subsystems.flywheel.FlywheelIOSim
 import com.team4099.robot2023.subsystems.flywheel.FlywheelIOTalon
 import com.team4099.robot2023.subsystems.intake.Intake
@@ -80,7 +82,7 @@ object RobotContainer {
       limelight = LimelightVision(LimelightVisionIOReal)
       intake = Intake(IntakeIOFalconNEO)
       feeder = Feeder(FeederIONeo)
-      elevator = Elevator(ElevatorIONEO)
+      elevator = Elevator(object: ElevatorIO {})
       flywheel = Flywheel(FlywheelIOTalon)
       wrist = Wrist(WristIOTalon)
     } else {

src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt

@@ -53,8 +53,8 @@ object Constants {
   }
 
   object Tuning {
-    const val TUNING_MODE = false
-    const val DEBUGING_MODE = false
+    const val TUNING_MODE = true
+    const val DEBUGING_MODE = true
     const val SIMULATE_DRIFT = false
     const val DRIFT_CONSTANT = 0.001
 

src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt

@@ -84,10 +84,10 @@ object DrivetrainConstants {
   val DRIVE_STATOR_THRESHOLD_CURRENT_LIMIT = 80.0.amps
   val DRIVE_STATOR_TRIGGER_THRESHOLD_TIME = 1.0.seconds
 
-  val FRONT_LEFT_MODULE_ZERO = 3.285.radians + PI.radians // good
-  val FRONT_RIGHT_MODULE_ZERO = 2.91.radians + PI.radians // good
-  val BACK_LEFT_MODULE_ZERO = 5.665.radians + PI.radians // good
-  val BACK_RIGHT_MODULE_ZERO = 3.50.radians + PI.radians // good
+  val FRONT_LEFT_MODULE_ZERO = 0.19.radians // good
+  val FRONT_RIGHT_MODULE_ZERO = 6.016.radians // good
+  val BACK_LEFT_MODULE_ZERO = 2.538.radians // good
+  val BACK_RIGHT_MODULE_ZERO = 1.25.radians // good
 
   val STEERING_COMPENSATION_VOLTAGE = 10.volts
   val DRIVE_COMPENSATION_VOLTAGE = 12.volts
@@ -184,4 +184,4 @@ object DrivetrainConstants {
     val SIM_STEERING_KI = 0.0.volts.perDegreeSeconds
     val SIM_STEERING_KD = 0.0.volts.perDegreePerSecond
   }
-}
+}

src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt

@@ -31,17 +31,12 @@ object WristConstants {
 
   val ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO = 69.0 / 20.0
   val MOTOR_TO_ABSOLUTE_ENCODER_GEAR_RATIO =
-    5.0 / 1.0 * 4.0 / 1.0 * 54.0 / 34.0 * 90.0 / 33.0 * 1.0 / (69.0 / 20.0)
+    5.0 / 1.0 * 4.0 / 1.0 * 54.0 / 18.0 * 48.0 / 18.0 * 1.0 / (69.0 / 20.0)
 
   val VOLTAGE_COMPENSATION = 12.0.volts
   val ABSOLUTE_ENCODER_OFFSET =
     (
-      97.72227856659904.degrees - 35.degrees + 1.90.degrees -
-        0.55.degrees -
-        0.6.degrees - // add to drop angle
-        1.degrees -
-        0.5.degrees -
-        96.3.degrees
+      -34.5.degrees + 3.95.degrees
       ) * ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO
   val WRIST_LENGTH = 18.6.inches
   val WRIST_INERTIA = 0.7181257183.kilo.grams * 1.0.meters.squared
@@ -59,31 +54,31 @@ object WristConstants {
 
   val WRIST_ZERO_SIM_OFFSET = 27.5.degrees
 
-  val MAX_WRIST_VELOCITY = 300.degrees.perSecond
-  val MAX_WRIST_ACCELERATION = 1500.degrees.perSecond.perSecond
+  val MAX_WRIST_VELOCITY = 400.degrees.perSecond
+  val MAX_WRIST_ACCELERATION = 400.degrees.perSecond.perSecond
 
   val HARDSTOP_OFFSET = 47.degrees
   object PID {
 
     val ARBITRARY_FEEDFORWARD = 0.03.volts
 
-    val REAL_KP: ProportionalGain<Radian, Volt> = 0.79.volts / 1.0.degrees
+    val REAL_KP: ProportionalGain<Radian, Volt> = 0.515.volts / 1.0.degrees
     val REAL_KI: IntegralGain<Radian, Volt> = 0.0.volts / (1.0.degrees * 1.0.seconds)
-    val REAL_KD: DerivativeGain<Radian, Volt> = 0.0.volts / 1.0.degrees.perSecond
+    val REAL_KD: DerivativeGain<Radian, Volt> = 0.03.volts / 1.0.degrees.perSecond
 
     val FIRST_STAGE_POS_SWITCH_THRESHOLD = 3.0.degrees
     val FIRST_STAGE_VEL_SWITCH_THRESHOLD = 3.0.degrees.perSecond
 
-    val FIRST_STAGE_KP: ProportionalGain<Radian, Volt> = 0.9.volts / 1.0.degrees
+    val FIRST_STAGE_KP: ProportionalGain<Radian, Volt> = 0.35.volts / 1.0.degrees
     val FIRST_STAGE_KI: IntegralGain<Radian, Volt> = 0.0.volts / (1.0.degrees * 1.0.seconds)
-    val FIRST_STAGE_KD: DerivativeGain<Radian, Volt> = 0.0.volts / 1.0.degrees.perSecond
+    val FIRST_STAGE_KD: DerivativeGain<Radian, Volt> = 0.03.volts / 1.0.degrees.perSecond
 
-    val SECOND_STAGE_POS_SWITCH_THRESHOLD = 1.0.degrees
+    val SECOND_STAGE_POS_SWITCH_THRESHOLD = 0.8.degrees
     val SECOND_STAGE_VEL_SWITCH_THRESHOLD = 5.0.degrees.perSecond
 
-    val SECOND_STAGE_KP: ProportionalGain<Radian, Volt> = 3.volts / 1.0.degrees
+    val SECOND_STAGE_KP: ProportionalGain<Radian, Volt> = 1.8.volts / 1.0.degrees
     val SECOND_STAGE_KI: IntegralGain<Radian, Volt> = 0.0.volts / (1.0.degrees * 1.0.seconds)
-    val SECOND_STAGE_KD: DerivativeGain<Radian, Volt> = 0.0.volts / 1.0.degrees.perSecond
+    val SECOND_STAGE_KD: DerivativeGain<Radian, Volt> = 0.075.volts / 1.0.degrees.perSecond
 
     val SIM_KP: ProportionalGain<Radian, Volt> = 10.volts / 1.0.degrees
     val SIM_KI: IntegralGain<Radian, Volt> = 0.0.volts / (1.0.degrees * 1.0.seconds)

src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt

@@ -80,7 +80,7 @@ class SwerveModuleIOTalon(
 
   private val potentiometerOutput: Double
     get() {
-      return if (label == Constants.Drivetrain.FRONT_RIGHT_MODULE_NAME) {
+      return if (label == Constants.Drivetrain.FRONT_RIGHT_MODULE_NAME || label == Constants.Drivetrain.BACK_RIGHT_MODULE_NAME) {
         potentiometer.voltage / RobotController.getVoltage5V() * 2.0 * PI
       } else {
         2 * PI - potentiometer.voltage / RobotController.getVoltage5V() * 2.0 * Math.PI
@@ -221,9 +221,9 @@ class SwerveModuleIOTalon(
     Logger.recordOutput(
       "$label/drivePosition",
       driveFalcon.position.value *
-        (PI) *
-        DrivetrainConstants.WHEEL_DIAMETER.inMeters *
-        DrivetrainConstants.DRIVE_SENSOR_GEAR_RATIO
+              (PI) *
+              DrivetrainConstants.WHEEL_DIAMETER.inMeters *
+              DrivetrainConstants.DRIVE_SENSOR_GEAR_RATIO
     )
     Logger.recordOutput("$label/drivePositionUnits", driveSensor.position.inMeters)
     inputs.drivePosition = driveSensor.position
@@ -341,9 +341,11 @@ class SwerveModuleIOTalon(
   override fun zeroSteering(isInAutonomous: Boolean) {
     steeringFalcon.setPosition(
       steeringSensor.positionToRawUnits(
-        if (label != Constants.Drivetrain.FRONT_RIGHT_MODULE_NAME)
+        if (label == Constants.Drivetrain.FRONT_RIGHT_MODULE_NAME || label == Constants.Drivetrain.BACK_RIGHT_MODULE_NAME){
+          (potentiometerOutput.radians - zeroOffset)
+        } else {
           (2 * PI).radians - (potentiometerOutput.radians - zeroOffset)
-        else (potentiometerOutput.radians - zeroOffset)
+        }
       )
     )
 
@@ -437,4 +439,4 @@ class SwerveModuleIOTalon(
       driveFalcon.setControl(DutyCycleOut(input.inVolts / 12.0))
     }
   }
-}
+}

src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt

@@ -379,7 +379,9 @@ class Wrist(val io: WristIO) : SubsystemBase() {
         }
         val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt
         val setPoint: TrapezoidProfile.State<Radian> = wristProfile.calculate(timeElapsed)
+
         setWristPosition(setPoint)
+
         Logger.recordOutput("Wrist/completedMotionProfile", wristProfile.isFinished(timeElapsed))
         Logger.recordOutput("Wrist/travelingUp", travelingUp)
         nextState = fromRequestToState(currentRequest)

src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt

@@ -148,6 +148,10 @@ object WristIOTalon : WristIO {
     wristConfiguration.Slot2.kD =
       wristSensor.derivativePositionGainToRawUnits(WristConstants.PID.SECOND_STAGE_KD)
 
+    wristConfiguration.Voltage.PeakForwardVoltage = 3.0
+    wristConfiguration.Voltage.PeakReverseVoltage = -3.0
+
+
     wristConfiguration.CurrentLimits.SupplyCurrentLimit =
       WristConstants.WRIST_SUPPLY_CURRENT_LIMIT.inAmperes
     wristConfiguration.CurrentLimits.SupplyCurrentThreshold =