merge shooter conflicts

simgui.json

@@ -1,9 +1,29 @@
 {
+  "HALProvider": {
+    "Other Devices": {
+      "window": {
+        "visible": false
+      }
+    }
+  },
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
       "/Shuffleboard/Pre-match/Mode": "String Chooser",
       "/SmartDashboard/AutonomousMode": "String Chooser"
     }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "SmartDashboard": {
+        "TunableNumbers": {
+          "Flywheel": {
+            "open": true
+          },
+          "open": true
+        },
+        "open": true
+      }
+    }
   }
 }

src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt

@@ -20,7 +20,7 @@ class PositionVoltage(
   private var overrideBrakeDurNeutral: Boolean = false,
   private var limitForwardMotion: Boolean = false,
   private var limitReverseMotion: Boolean = false,
-  private var velocity: AngularVelocity = 0.0.degrees.perSecond,
+  private var velocity: AngularVelocity = 0.0.degrees.perSecond
 ) {
 
   val positionVoltagePhoenix6 =

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

@@ -15,10 +15,16 @@ 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.FlywheelIOSim
+import com.team4099.robot2023.subsystems.flywheel.FlywheelIOTalon
 import com.team4099.robot2023.subsystems.limelight.LimelightVision
 import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO
 import com.team4099.robot2023.subsystems.vision.Vision
 import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar
+import com.team4099.robot2023.subsystems.wrist.Wrist
+import com.team4099.robot2023.subsystems.wrist.WristIONeo
+import com.team4099.robot2023.subsystems.wrist.WristIOSim
 import com.team4099.robot2023.util.driver.Ryan
 import edu.wpi.first.wpilibj.RobotBase
 import org.team4099.lib.smoothDeadband
@@ -32,6 +38,8 @@ object RobotContainer {
   private val limelight: LimelightVision
   private val feeder: Feeder
   private val elevator: Elevator
+  private val flywheel: Flywheel
+  private val wrist: Wrist
 
   init {
     if (RobotBase.isReal()) {
@@ -51,6 +59,8 @@ object RobotContainer {
       limelight = LimelightVision(object : LimelightVisionIO {})
       feeder = Feeder(FeederIONeo)
       elevator = Elevator(ElevatorIONEO)
+      flywheel = Flywheel(FlywheelIOTalon)
+      wrist = Wrist(WristIONeo)
     } else {
       // Simulation implementations
       drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim)
@@ -63,6 +73,8 @@ object RobotContainer {
       limelight = LimelightVision(object : LimelightVisionIO {})
       feeder = Feeder(FeederIOSim)
       elevator = Elevator(ElevatorIOSim)
+      flywheel = Flywheel(FlywheelIOSim)
+      wrist = Wrist(WristIOSim)
     }
 
     vision.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) })

src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt

@@ -239,11 +239,6 @@ class DrivePathCommand(
     drivetrain.targetPose =
       Pose2d(Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position))
 
-    Logger.recordOutput(
-      "Pathfollow/target",
-      Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position)
-    )
-
     drivetrain.currentRequest =
       DrivetrainRequest.ClosedLoop(
         nextDriveState,
@@ -275,7 +270,6 @@ class DrivePathCommand(
       desiredRotation.velocityRadiansPerSec.radians.perSecond.inDegreesPerSecond
     )
 
-    Logger.recordOutput("Pathfollow/trajectory", trajectory)
     Logger.recordOutput("Pathfollow/isAtReference", swerveDriveController.atReference())
     Logger.recordOutput("Pathfollow/trajectoryTimeSeconds", trajectory.totalTimeSeconds)
 

src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt

@@ -94,6 +94,4 @@ object ControlBoard {
   val extendIntake = Trigger { technician.aButton }
   val retractIntake = Trigger { technician.bButton }
   val characterizeIntake = Trigger { technician.xButton }
-
-  // for testing
 }

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

@@ -116,10 +116,13 @@ object Constants {
   }
 
   object Shooter {
-    const val FLYWHEEL_MOTOR_ID = 51
-    // TODO find wrist motor id
-    const val SHOOTER_WRIST_MOTOR_ID = 999
-    const val FEEDER_MOTOR_ID = 999
+    const val FLYWHEEL_LEFT_MOTOR_ID = 51
+    const val FLYWHEEL_RIGHT_MOTOR_ID = 52
+  }
+
+  object WRIST {
+    const val WRIST_MOTOR_ID = 41
+    const val CANCODER_ID = 42
   }
 
   object Alert {

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

@@ -0,0 +1,85 @@
+package com.team4099.robot2023.config.constants
+
+import org.team4099.lib.units.Velocity
+import org.team4099.lib.units.base.amps
+import org.team4099.lib.units.base.grams
+import org.team4099.lib.units.base.meters
+import org.team4099.lib.units.base.seconds
+import org.team4099.lib.units.derived.DerivativeGain
+import org.team4099.lib.units.derived.IntegralGain
+import org.team4099.lib.units.derived.ProportionalGain
+import org.team4099.lib.units.derived.Radian
+import org.team4099.lib.units.derived.Volt
+import org.team4099.lib.units.derived.radians
+import org.team4099.lib.units.derived.rotations
+import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.kilo
+import org.team4099.lib.units.perMinute
+import org.team4099.lib.units.perSecond
+
+object FlywheelConstants {
+  val LEFT_GEAR_RATIO = 1.0
+
+  val RIGHT_MOTOR_REVOLUTIONS_PER_FLYWHEEL_REVOLUTIONS = 24.0 / 48.0
+
+  val VOLTAGE_COMPENSATION = 12.volts
+
+  val INERTIA = 0.0014550597.kilo.grams * 1.0.meters.squared
+
+  val RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT = 50.0.amps
+  val RIGHT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT = 1.0.amps
+  val RIGHT_flywheel_TRIGGER_THRESHOLD_TIME = 10.0.seconds
+  val RIGHT_FLYWHEEL_STATOR_CURRENT_LIMIT = 50.0.amps
+
+  val LEFT_FLYWHEEL_SUPPLY_CURRENT_LIMIT = 50.0.amps
+  val LEFT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT = 1.0.amps
+  val LEFT_flywheel_TRIGGER_THRESHOLD_TIME = 10.0.seconds
+  val LEFT_FLYWHEEL_STATOR_CURRENT_LIMIT = 50.0.amps
+
+  val FLYWHEEL_TOLERANCE = 50.0.rotations.perMinute
+  object PID {
+    val RIGHT_REAL_KP: ProportionalGain<Velocity<Radian>, Volt> =
+      0.01.volts / 1.0.rotations.perMinute
+    val RIGHT_REAL_KI: IntegralGain<Velocity<Radian>, Volt> =
+      0.0.volts / (1.0.rotations.perMinute * 1.0.seconds)
+    val RIGHT_REAL_KD: DerivativeGain<Velocity<Radian>, Volt> =
+      0.0.volts / (1.0.rotations.perMinute.perSecond)
+
+    val RIGHT_SIM_KP: ProportionalGain<Velocity<Radian>, Volt> =
+      0.015.volts / 1.0.rotations.perMinute
+    val RIGHT_SIM_KI: IntegralGain<Velocity<Radian>, Volt> =
+      0.0.volts / (1.0.rotations.perMinute * 1.0.seconds)
+    val RIGHT_SIM_KD: DerivativeGain<Velocity<Radian>, Volt> =
+      0.0.volts / (1.0.rotations.perMinute.perSecond)
+
+    val LEFT_REAL_KP: ProportionalGain<Velocity<Radian>, Volt> =
+      0.015.volts / 1.0.rotations.perMinute
+    val LEFT_REAL_KI: IntegralGain<Velocity<Radian>, Volt> =
+      0.0.volts / (1.0.rotations.perMinute * 1.0.seconds)
+    val LEFT_REAL_KD: DerivativeGain<Velocity<Radian>, Volt> =
+      0.0.volts / (1.0.rotations.perMinute.perSecond)
+
+    val LEFT_SIM_KP: ProportionalGain<Velocity<Radian>, Volt> =
+      0.0015.volts / 1.0.rotations.perMinute
+    val LEFT_SIM_KI: IntegralGain<Velocity<Radian>, Volt> =
+      0.0.volts / (1.0.rotations.perMinute * 1.0.seconds)
+    val LEFT_SIM_KD: DerivativeGain<Velocity<Radian>, Volt> =
+      0.0.volts / (1.0.rotations.perMinute.perSecond)
+
+    val RIGHT_REAL_FLYWHEEL_KS = 0.0.volts
+    val RIGHT_REAL_FLYWHEEL_KV = 0.0099.volts / 1.radians.perSecond
+    val RIGHT_REAL_FLYWHEEL_KA = 0.09.volts / 1.radians.perSecond.perSecond
+
+    val LEFT_REAL_FLYWHEEL_KS = 0.0.volts
+    val LEFT_REAL_FLYWHEEL_KV = 0.0197.volts / 1.radians.perSecond
+    val LEFT_REAL_FLYWHEEL_KA = 0.03.volts / 1.radians.perSecond.perSecond
+
+    val RIGHT_SIM_FLYWHEEL_KS = 0.0.volts
+    val RIGHT_SIM_FLYWHEEL_KV = 0.0099.volts / 1.radians.perSecond
+    val RIGHT_SIM_FLYWHEEL_KA = 0.09.volts / 1.radians.perSecond.perSecond
+
+    val LEFT_SIM_FLYWHEEL_KS = 0.0.volts
+    val LEFT_SIM_FLYWHEEL_KV = 0.0197.volts / 1.radians.perSecond
+    val LEFT_SIM_FLYWHEEL_KA = 0.03.volts / 1.radians.perSecond.perSecond
+  }
+}

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

@@ -0,0 +1,57 @@
+package com.team4099.robot2023.config.constants
+
+import org.team4099.lib.units.base.amps
+import org.team4099.lib.units.base.grams
+import org.team4099.lib.units.base.inches
+import org.team4099.lib.units.base.meters
+import org.team4099.lib.units.base.seconds
+import org.team4099.lib.units.derived.DerivativeGain
+import org.team4099.lib.units.derived.IntegralGain
+import org.team4099.lib.units.derived.ProportionalGain
+import org.team4099.lib.units.derived.Radian
+import org.team4099.lib.units.derived.Volt
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.radians
+import org.team4099.lib.units.derived.rotations
+import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.kilo
+import org.team4099.lib.units.perSecond
+
+object WristConstants {
+  // val ROLLER_GEAR_RATIO = 0.0
+  // val ROLLER_VOLTAGE_COMPENSATION = 0.0.volts
+  // val ROLLER_STATOR_CURRENT_LIMIT = 0.0.amps
+
+  val VOLTAGE_COMPENSATION = 12.0.volts
+  val ABSOLUTE_ENCODER_OFFSET = 0.degrees
+  val WRIST_LENGTH = 18.6.inches
+  val WRIST_INERTIA = 0.7181257183.kilo.grams * 1.0.meters.squared
+
+  val WRIST_ENCODER_GEAR_RATIO = 0.0
+
+  val WRIST_GEAR_RATIO = 90.0 / 12 * 90 / 24 * 90 / 30
+  val WRIST_VOLTAGE_COMPENSATION = 12.0.volts
+  val WRIST_STATOR_CURRENT_LIMIT = 40.0.amps
+
+  val WRIST_MAX_ROTATION = 45.degrees
+  val WRIST_MIN_ROTATION = (-47.0).degrees
+
+  val MAX_WRIST_VELOCITY = 1.radians.perSecond
+  val MAX_WRIST_ACCELERATION = 2.radians.perSecond.perSecond
+  object PID {
+    val REAL_KP: ProportionalGain<Radian, Volt> = 0.001.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.rotations / 1.0.seconds)
+
+    val SIM_KP: ProportionalGain<Radian, Volt> = 1.volts / 1.0.degrees
+    val SIM_KI: IntegralGain<Radian, Volt> = 0.0.volts / (1.0.degrees * 1.0.seconds)
+    val SIM_KD: DerivativeGain<Radian, Volt> = 0.00.volts / (1.0.degrees / 1.0.seconds)
+
+    val WRIST_KG = 1.25.volts
+    val WRIST_KV = 1.61.volts / 1.0.radians.perSecond
+    val WRIST_KA = 0.03.volts / 1.0.radians.perSecond.perSecond
+    val WRIST_KS = 0.0.volts
+  }
+
+  val WRIST_TOLERANCE = 0.01.degrees
+}

src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt

@@ -169,6 +169,7 @@ class SwerveModule(val io: SwerveModuleIO) {
       (steering - inputs.steeringPosition).inRadians.IEEErem(2 * Math.PI).radians
 
     shouldInvert = (steeringDifference.absoluteValue > (Math.PI / 2).radians) && optimize
+
     if (shouldInvert) {
       steeringDifference -= Math.PI.withSign(steeringDifference.inRadians).radians
     }