shooter characterization configuration

blair-robot-project · Feb 13, 2024 · 43eb52b · 43eb52b
1 parent 6504470
commit 43eb52b
Show file tree

Hide file tree

Showing 10 changed files with 113 additions and 56 deletions.
diff --git a/.run/Build & Deploy Robot.run.xml b/.run/Build & Deploy Robot.run.xml
@@ -18,7 +18,7 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <ForceTestExec>false</ForceTestExec>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
   <configuration default="false" name="Build &amp; Deploy Robot" type="GradleRunConfiguration" factoryName="Gradle">
@@ -40,7 +40,7 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <ForceTestExec>false</ForceTestExec>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
   <configuration default="false" name="Build &amp; Deploy Robot" type="GradleRunConfiguration" factoryName="Gradle">
@@ -62,7 +62,7 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <ForceTestExec>false</ForceTestExec>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
   <configuration default="false" name="Build &amp; Deploy Robot" type="GradleRunConfiguration" factoryName="Gradle">
@@ -84,15 +84,15 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <ForceTestExec>false</ForceTestExec>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
   <configuration default="false" name="Build &amp; Deploy Robot" type="GradleRunConfiguration" factoryName="Gradle">
     <ExternalSystemSettings>
       <option name="executionName" />
       <option name="externalProjectPath" value="$PROJECT_DIR$" />
       <option name="externalSystemIdString" value="GRADLE" />
-      <option name="scriptParameters" />
+      <option name="scriptParameters" value="" />
       <option name="taskDescriptions">
         <list />
       </option>
@@ -106,7 +106,7 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <ForceTestExec>false</ForceTestExec>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
 </component>
diff --git a/.run/Build Robot.run.xml b/.run/Build Robot.run.xml
@@ -18,7 +18,7 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <ForceTestExec>false</ForceTestExec>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
   <configuration default="false" name="Build Robot" type="GradleRunConfiguration" factoryName="Gradle">
@@ -40,7 +40,7 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <ForceTestExec>false</ForceTestExec>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
   <configuration default="false" name="Build Robot" type="GradleRunConfiguration" factoryName="Gradle">
@@ -62,7 +62,7 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <ForceTestExec>false</ForceTestExec>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
   <configuration default="false" name="Build Robot" type="GradleRunConfiguration" factoryName="Gradle">
@@ -84,15 +84,15 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <ForceTestExec>false</ForceTestExec>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
   <configuration default="false" name="Build Robot" type="GradleRunConfiguration" factoryName="Gradle">
     <ExternalSystemSettings>
       <option name="executionName" />
       <option name="externalProjectPath" value="$PROJECT_DIR$" />
       <option name="externalSystemIdString" value="GRADLE" />
-      <option name="scriptParameters" />
+      <option name="scriptParameters" value="" />
       <option name="taskDescriptions">
         <list />
       </option>
@@ -106,7 +106,7 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <ForceTestExec>false</ForceTestExec>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
 </component>
diff --git a/.run/Clean Build & Deploy Robot.run.xml b/.run/Clean Build & Deploy Robot.run.xml
@@ -19,7 +19,7 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <ForceTestExec>false</ForceTestExec>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
   <configuration default="false" name="Clean Build &amp; Deploy Robot" type="GradleRunConfiguration" factoryName="Gradle">
@@ -42,7 +42,7 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <ForceTestExec>false</ForceTestExec>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
   <configuration default="false" name="Clean Build &amp; Deploy Robot" type="GradleRunConfiguration" factoryName="Gradle">
@@ -65,7 +65,7 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <ForceTestExec>false</ForceTestExec>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
   <configuration default="false" name="Clean Build &amp; Deploy Robot" type="GradleRunConfiguration" factoryName="Gradle">
@@ -88,15 +88,15 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <ForceTestExec>false</ForceTestExec>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
   <configuration default="false" name="Clean Build &amp; Deploy Robot" type="GradleRunConfiguration" factoryName="Gradle">
     <ExternalSystemSettings>
       <option name="executionName" />
       <option name="externalProjectPath" value="$PROJECT_DIR$" />
       <option name="externalSystemIdString" value="GRADLE" />
-      <option name="scriptParameters" />
+      <option name="scriptParameters" value="" />
       <option name="taskDescriptions">
         <list />
       </option>
@@ -111,7 +111,7 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <ForceTestExec>false</ForceTestExec>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
 </component>
diff --git a/.run/Clean.run.xml b/.run/Clean.run.xml
@@ -18,7 +18,7 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <ForceTestExec>false</ForceTestExec>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
   <configuration default="false" name="Clean" type="GradleRunConfiguration" factoryName="Gradle">
@@ -40,7 +40,7 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <ForceTestExec>false</ForceTestExec>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
   <configuration default="false" name="Clean" type="GradleRunConfiguration" factoryName="Gradle">
@@ -62,7 +62,7 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <ForceTestExec>false</ForceTestExec>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
   <configuration default="false" name="Clean" type="GradleRunConfiguration" factoryName="Gradle">
@@ -84,15 +84,15 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <ForceTestExec>false</ForceTestExec>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
   <configuration default="false" name="Clean" type="GradleRunConfiguration" factoryName="Gradle">
     <ExternalSystemSettings>
       <option name="executionName" />
       <option name="externalProjectPath" value="$PROJECT_DIR$" />
       <option name="externalSystemIdString" value="GRADLE" />
-      <option name="scriptParameters" />
+      <option name="scriptParameters" value="" />
       <option name="taskDescriptions">
         <list />
       </option>
@@ -106,7 +106,7 @@
     <ExternalSystemDebugServerProcess>true</ExternalSystemDebugServerProcess>
     <ExternalSystemReattachDebugProcess>true</ExternalSystemReattachDebugProcess>
     <DebugAllEnabled>false</DebugAllEnabled>
-    <ForceTestExec>false</ForceTestExec>
+    <RunAsTest>false</RunAsTest>
     <method v="2" />
   </configuration>
 </component>
diff --git a/simgui-ds.json b/simgui-ds.json
@@ -90,9 +90,6 @@
     }
   ],
   "robotJoysticks": [
-    {
-      "useGamepad": true
-    },
     {
       "guid": "Keyboard0"
     }

diff --git a/simgui.json b/simgui.json
@@ -355,7 +355,7 @@
             0.0,
             0.8500000238418579
           ],
-          "height": 599,
+          "height": 0,
           "series": [
             {
               "color": [

diff --git a/src/main/kotlin/frc/team449/robot2024/constants/subsystem/ShooterConstants.kt b/src/main/kotlin/frc/team449/robot2024/constants/subsystem/ShooterConstants.kt
@@ -54,8 +54,10 @@ object ShooterConstants {
   /** Encoder stuff */
   const val LEFT_CHANNEL_A = 1
   const val LEFT_CHANNEL_B = 2
-  const val RIGHT_CHANNEL_A = 4
-  const val RIGHT_CHANNEL_B = 3
+
+  // check
+  const val RIGHT_CHANNEL_A = 3
+  const val RIGHT_CHANNEL_B = 4
   const val CPR = 2048
   const val UPR = 2 * PI
   const val GEARING = 1.0 / 1.0

diff --git a/src/main/kotlin/frc/team449/robot2024/subsystems/ControllerBindings.kt b/src/main/kotlin/frc/team449/robot2024/subsystems/ControllerBindings.kt
@@ -2,10 +2,12 @@ package frc.team449.robot2024.subsystems
 
 import edu.wpi.first.math.geometry.Rotation2d
 import edu.wpi.first.units.Measure
-import edu.wpi.first.units.Units.Volts
+import edu.wpi.first.units.MutableMeasure.mutable
+import edu.wpi.first.units.Units.*
 import edu.wpi.first.units.Voltage
 import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj.RobotBase
+import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog
 import edu.wpi.first.wpilibj2.command.*
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController
 import edu.wpi.first.wpilibj2.command.button.Trigger
@@ -21,6 +23,7 @@ import kotlin.math.PI
 import kotlin.math.abs
 import kotlin.math.pow
 
+
 class ControllerBindings(
   private val driveController: CommandXboxController,
   private val mechanismController: CommandXboxController,
@@ -36,6 +39,56 @@ class ControllerBindings(
     )
   )
 
+  private val m_appliedVoltage = mutable(Volts.of(0.0))
+  private val m_angle = mutable(Rotations.of(0.0))
+  private val m_velocity = mutable(RotationsPerSecond.of(0.0))
+
+  val shooterRoutine = SysIdRoutine(
+    SysIdRoutine.Config(),
+    Mechanism(
+      { voltage: Measure<Voltage> ->
+        run {
+          robot.shooter.setLeftVoltage(voltage.`in`(Volts))
+          robot.shooter.setRightVoltage(voltage.`in`(Volts))
+        }
+      },
+      { log: SysIdRoutineLog ->
+        run {
+          log.motor("shooter-left")
+            .voltage(
+              m_appliedVoltage.mut_replace(
+                robot.shooter.leftMotor.get() * robot.powerDistribution.voltage, Volts
+              )
+            )
+            .angularPosition(
+              m_angle.mut_replace(
+                robot.shooter.leftMotor.position, Rotations
+              )
+            )
+            .angularVelocity(
+              m_velocity.mut_replace(robot.shooter.leftVelocity.get(), RotationsPerSecond)
+            )
+          log.motor("shooter-right")
+            .voltage(
+              m_appliedVoltage.mut_replace(
+                robot.shooter.rightMotor.get() * robot.powerDistribution.voltage, Volts
+              )
+            )
+            .angularPosition(
+              m_angle.mut_replace(
+                robot.shooter.rightMotor.position, Rotations
+              )
+            )
+            .angularVelocity(
+              m_velocity.mut_replace(robot.shooter.rightVelocity.get(), RotationsPerSecond)
+            )
+        }
+      },
+      robot.shooter,
+      "shooter"
+    )
+  )
+
   val orbitCmd = OrbitAlign(
     robot.drive,
     robot.driveController.hid,
@@ -129,28 +182,28 @@ class ControllerBindings(
       )
     )
 
-//    /** Characterization */
-//    // Quasistatic Forwards
-//    driveController.povUp().onTrue(
-//      sysIdRoutine.quasistatic(SysIdRoutine.Direction.kForward)
-//    )
-//
-//    // Quasistatic Reverse
-//    driveController.povDown().onTrue(
-//      sysIdRoutine.quasistatic(SysIdRoutine.Direction.kReverse)
-//    )
-//
-//    // Dynamic Forwards
-//    driveController.povRight().onTrue(
-//      sysIdRoutine.dynamic(SysIdRoutine.Direction.kForward)
-//    )
-//
-//    // Dynamic Reverse
-//    driveController.povLeft().onTrue(
-//      sysIdRoutine.dynamic(SysIdRoutine.Direction.kReverse)
-//    )
+    /** Characterization */
+    // Quasistatic Forwards
+    driveController.povUp().onTrue(
+      shooterRoutine.quasistatic(SysIdRoutine.Direction.kForward)
+    )
+
+    // Quasistatic Reverse
+    driveController.povDown().onTrue(
+      shooterRoutine.quasistatic(SysIdRoutine.Direction.kReverse)
+    )
+
+    // Dynamic Forwards
+    driveController.povRight().onTrue(
+      shooterRoutine.dynamic(SysIdRoutine.Direction.kForward)
+    )
+
+    // Dynamic Reverse
+    driveController.povLeft().onTrue(
+      shooterRoutine.dynamic(SysIdRoutine.Direction.kReverse)
+    )
 
-    /** Shooting from anywhere */
+/** Shooting from anywhere */
 //    mechanismController.b().onTrue(
 //      ParallelCommandGroup(
 //        orbitCmd,
-Original file line number
+Diff line change
@@ Expand Up / @@ -90,9 +90,6 @@ @@
         }
       ],
       "robotJoysticks": [
-        {
-          "useGamepad": true
-        },
         {
           "guid": "Keyboard0"
         }
@@ Expand Down @@