Inverse gearing for state space and profile, assign animation for dis…

…able, teleop, auto, extend intake, running intake

simgui-ds.json

@@ -90,6 +90,7 @@
     }
   ],
   "robotJoysticks": [
+    {},
     {
       "guid": "Keyboard0"
     }

simgui.json

@@ -105,10 +105,14 @@
               "open": true
             }
           },
+          "elevator": {
+            "open": true
+          },
           "intake": {
             "double[]##v_/Monologuing/robot/intake/3.1 Intake 3D Position": {
               "open": true
-            }
+            },
+            "open": true
           },
           "open": true
         }
@@ -152,7 +156,7 @@
             0.0,
             0.8500000238418579
           ],
-          "height": 926,
+          "height": 306,
           "series": [
             {
               "color": [

src/main/kotlin/frc/team449/RobotLoop.kt

@@ -5,9 +5,12 @@ import edu.wpi.first.wpilibj.RobotBase
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.CommandScheduler
+import edu.wpi.first.wpilibj2.command.InstantCommand
 import frc.team449.robot2023.Robot
 import frc.team449.robot2023.auto.routines.RoutineChooser
 import frc.team449.robot2023.commands.light.BlairChasing
+import frc.team449.robot2023.commands.light.BreatheHue
+import frc.team449.robot2023.commands.light.Rainbow
 import frc.team449.robot2023.constants.vision.VisionConstants
 import frc.team449.robot2023.subsystems.ControllerBindings
 import monologue.Logged
@@ -50,11 +53,11 @@ class RobotLoop : TimedRobot(), Logged {
     SmartDashboard.putData("Command Scheduler", CommandScheduler.getInstance())
     SmartDashboard.putBoolean("Enable Logging?", false)
 
+    robot.light.defaultCommand = BlairChasing(robot.light)
+
     controllerBinder.bindButtons()
 
     Monologue.setupLogging(this, "/Monologuing")
-
-    robot.light.defaultCommand = BlairChasing(robot.light)
   }
 
   override fun robotPeriodic() {
@@ -77,6 +80,12 @@ class RobotLoop : TimedRobot(), Logged {
     /** Every time auto starts, we update the chosen auto command */
     this.autoCommand = routineMap[routineChooser.selected]
     CommandScheduler.getInstance().schedule(this.autoCommand)
+
+    if (DriverStation.getAlliance().getOrNull() == DriverStation.Alliance.Red) {
+      BreatheHue(robot.light, 0).schedule()
+    } else {
+      BreatheHue(robot.light, 95).schedule()
+    }
   }
 
   override fun autonomousPeriodic() {}
@@ -88,6 +97,8 @@ class RobotLoop : TimedRobot(), Logged {
       CommandScheduler.getInstance().cancel(autoCommand)
     }
 
+    (robot.light.currentCommand?: InstantCommand()).cancel()
+
     robot.drive.defaultCommand = robot.driveCommand
   }
 
@@ -97,6 +108,8 @@ class RobotLoop : TimedRobot(), Logged {
   override fun disabledInit() {
     robot.drive.stop()
 
+    Rainbow(robot.light).schedule()
+
 //    if (VisionConstants.ESTIMATORS.isEmpty()) {
 //      VisionConstants.ESTIMATORS.add(
 //        VisionEstimator(

src/main/kotlin/frc/team449/control/TrapezoidalExponentialProfile.kt

@@ -11,15 +11,15 @@ class TrapezoidalExponentialProfile(
   pulleyRadius: Double = ElevatorConstants.PULLEY_RADIUS,
   currentLimit: Int = ElevatorConstants.CURRENT_LIMIT,
   numMotors: Int = ElevatorConstants.NUM_MOTORS,
-  effectiveGearing: Double = ElevatorConstants.EFFECTIVE_GEARING,
+  effectiveGearing: Double = 1 / ElevatorConstants.EFFECTIVE_GEARING,
   systemMass: Double = ElevatorConstants.CARRIAGE_MASS,
   angle: Double = ElevatorConstants.ANGLE,
   private val tolerance: Double = 0.05,
   private val vMax: Double = 5.0,
   startingDistance: Double = 0.0,
   private var finalDistance: Double,
   private var aStop: Double = 9.81,
-  private val efficiency: Double = ElevatorConstants.EFFICIENCY
+  efficiency: Double = ElevatorConstants.EFFICIENCY
 ) {
   private val trueStartingDistance: Double = startingDistance
   private val trueFinalDistance: Double = finalDistance

src/main/kotlin/frc/team449/robot2023/commands/light/SolidColor.kt

@@ -3,8 +3,7 @@ package frc.team449.robot2023.commands.light
 import edu.wpi.first.wpilibj2.command.Command
 import frc.team449.robot2023.subsystems.light.Light
 
-/** Description: Have a random color set for every led every 20ms
- *    a.k.a. you go blind  */
+/** Description: Have a solid color  */
 class SolidColor(
   private val led: Light,
   private val r: Int,

src/main/kotlin/frc/team449/robot2023/constants/subsystem/ElevatorConstants.kt

@@ -23,7 +23,7 @@ object ElevatorConstants {
   var UPR = PULLEY_RADIUS * 2 * PI
 
   var kS = 0.0
-  var kG = 1.26725
+  var kG = 1.085
 
   const val HIGH_DISTANCE = 1.114425
   const val LOW_DISTANCE = 0.25

src/main/kotlin/frc/team449/robot2023/subsystems/ControllerBindings.kt

@@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton
 import edu.wpi.first.wpilibj2.command.button.Trigger
 import frc.team449.robot2023.Robot
 import frc.team449.robot2023.commands.characterization.Characterization
-import frc.team449.robot2023.commands.light.SolidColor
+import frc.team449.robot2023.commands.light.BreatheHue
 import kotlin.math.abs
 
 class ControllerBindings(
@@ -29,7 +29,7 @@ class ControllerBindings(
 
     JoystickButton(mechanismController, XboxController.Button.kY.value).onTrue(
       SequentialCommandGroup(
-        robot.intake.extend(),
+//        robot.intake.extend(),
         robot.elevator.high()
       )
     )
@@ -70,15 +70,17 @@ class ControllerBindings(
       )
     )
 
-    Trigger { driveController.rightTriggerAxis > 0.8 }.onTrue(
+    Trigger { mechanismController.rightTriggerAxis > 0.8 }.onTrue(
       ParallelCommandGroup(
         robot.intake.intake(),
-        robot.manipulator.intake()
+        robot.manipulator.intake(),
+        BreatheHue(robot.light, 30)
       )
     ).onFalse(
       ParallelCommandGroup(
         robot.intake.stop(),
-        robot.manipulator.stop()
+        robot.manipulator.stop(),
+        robot.light.defaultCommand
       )
     )
 
@@ -88,20 +90,23 @@ class ControllerBindings(
       robot.elevator.defaultCommand
     )
 
-    Trigger { driveController.leftTriggerAxis > 0.8 }.onTrue(
+    Trigger { mechanismController.leftTriggerAxis > 0.8 }.onTrue(
       ParallelCommandGroup(
         robot.intake.outtake(),
-        robot.manipulator.outtake()
+        robot.manipulator.outtake(),
+        BreatheHue(robot.light, 160)
+
       )
     ).onFalse(
       ParallelCommandGroup(
         robot.intake.stop(),
-        robot.manipulator.stop()
+        robot.manipulator.stop(),
+        robot.light.defaultCommand
       )
     )
 
     Trigger { robot.intake.piston.get() == DoubleSolenoid.Value.kForward }.onTrue(
-      SolidColor(robot.light, 0, 255, 0)
+      BreatheHue(robot.light, 60)
     ).onFalse(
       robot.light.defaultCommand
     )

src/main/kotlin/frc/team449/robot2023/subsystems/elevator/Elevator.kt

@@ -149,7 +149,9 @@ open class Elevator(
           loop.predict(ElevatorConstants.DT)
 
           motor.setVoltage(
-            loop.getU(0) + ElevatorConstants.kG + sign(desiredState.second) * ElevatorConstants.kS
+            loop.getU(0) +
+              ElevatorConstants.kG +
+              sign(desiredState.second) * ElevatorConstants.kS
           )
 
           dtList.add(dt)
@@ -268,7 +270,7 @@ open class Elevator(
         ),
         ElevatorConstants.CARRIAGE_MASS,
         ElevatorConstants.PULLEY_RADIUS,
-        ElevatorConstants.EFFECTIVE_GEARING
+        1 / ElevatorConstants.EFFECTIVE_GEARING
       )
 
       val observer = KalmanFilter(

src/main/kotlin/frc/team449/robot2023/subsystems/elevator/ElevatorSim.kt

@@ -32,7 +32,7 @@ class ElevatorSim(
       MotorConstants.FREE_SPEED,
       ElevatorConstants.NUM_MOTORS
     ),
-    ElevatorConstants.EFFECTIVE_GEARING,
+    1 / ElevatorConstants.EFFECTIVE_GEARING,
     ElevatorConstants.CARRIAGE_MASS,
     ElevatorConstants.PULLEY_RADIUS,
     0.0,