tested simulation

simgui-ds.json

@@ -88,5 +88,10 @@
       "buttonCount": 0,
       "povCount": 0
     }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "Keyboard0"
+    }
   ]
 }

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

@@ -6,10 +6,10 @@ package com.team4099.robot2023
 const val MAVEN_GROUP = ""
 const val MAVEN_NAME = "Crescendo-2024"
 const val VERSION = "unspecified"
-const val GIT_REVISION = 45
-const val GIT_SHA = "5e1ed64a8159953426ea83a1f210857ac81b7ef8"
-const val GIT_DATE = "2024-01-19T22:05:22Z"
+const val GIT_REVISION = 52
+const val GIT_SHA = "7c071041850824a24f844161f49b8016c9b1b145"
+const val GIT_DATE = "2024-01-23T18:32:44Z"
 const val GIT_BRANCH = "elevator"
-const val BUILD_DATE = "2024-01-19T22:38:32Z"
-const val BUILD_UNIX_TIME = 1705721912200L
+const val BUILD_DATE = "2024-01-23T20:24:59Z"
+const val BUILD_UNIX_TIME = 1706059499671L
 const val DIRTY = 1

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

@@ -137,7 +137,7 @@ object RobotContainer {
     ControlBoard.elevatorOpenLoopExtend.whileTrue(elevator.testElevatorOpenLoopExtendCommand())
     ControlBoard.elevatorOpenLoopRetract.whileTrue(elevator.testElevatorOpenLoopRetractCommand())
     ControlBoard.elevatorClosedLoopHigh.whileTrue(elevator.testElevatorClosedLoopExtendCommand())
-    ControlBoard.elevatorClosedLoopLow.whileTrue(elevator.testElevatorClosedLoopExtendCommand())
+    ControlBoard.elevatorClosedLoopLow.whileTrue(elevator.elevatorClosedLoopRetractCommand())
   }
 
   fun mapTestControls() {}

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

@@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.Command
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.controller.ProfiledPIDController
 import org.team4099.lib.controller.TrapezoidProfile
+import org.team4099.lib.units.Fraction
 import org.team4099.lib.units.Value
 import org.team4099.lib.units.Velocity
 import org.team4099.lib.units.base.Meter

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

@@ -21,7 +21,7 @@ object ElevatorConstants {
   val REAL_KI = 0.0.volts / (1.inches * 1.seconds)
   val REAL_KD = 0.0.volts / (1.inches.perSecond)
 
-  val CARRIAGE_MASS = 20.pounds
+  val CARRIAGE_MASS = 30.892.pounds
 
   val ELEVATOR_MAX_RETRACTION = 0.0.inches
   val ELEVATOR_MAX_EXTENSION = 18.0.inches
@@ -37,30 +37,30 @@ object ElevatorConstants {
   val FOLLOWER_KI: IntegralGain<Meter, Volt> = 0.0.volts / (1.inches * 1.seconds)
   val FOLLOWER_KD: DerivativeGain<Meter, Volt> = 0.0.volts / (1.inches.perSecond)
 
-  val SIM_KP = 0.0.volts / 1.inches
+  val SIM_KP = 2.0.volts / 1.inches
   val SIM_KI = 0.0.volts / (1.inches * 1.seconds)
   val SIM_KD = 0.0.volts / (1.inches.perSecond)
 
   val ELEVATOR_KS = 0.0.volts
-  val ELEVATOR_KG = 0.0.volts
-  val ELEVATOR_KV = 0.0.volts / 0.0.inches.perSecond
-  val ELEVATOR_KA = 0.0.volts / 0.0.inches.perSecond.perSecond
+  val ELEVATOR_KG = 0.32.volts
+  val ELEVATOR_KV = 0.39.volts / 1.inches.perSecond
+  val ELEVATOR_KA = 0.00083.volts / 1.inches.perSecond.perSecond
   val ELEVATOR_OPEN_LOOP_EXTEND_VOLTAGE = 8.0.volts
   val ELEVATOR_OPEN_LOOP_RETRACT_VOLTAGE = -12.0.volts
 
   val ENABLE_ELEVATOR = false
   val ELEVATOR_IDLE_HEIGHT = 0.0.inches
-  val ELEVATOR_SOFT_LIMIT_EXTENSION = 0.0.inches
-  val ELEVATOR_SOFT_LIMIT_RETRACTION = 0.0.inches
+  val ELEVATOR_SOFT_LIMIT_EXTENSION = 17.5.inches
+  val ELEVATOR_SOFT_LIMIT_RETRACTION = -1.0.inches
   val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_EXTENSION = 0.0.inches
   val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_RETRACTION = 0.0.inches
   val ELEVATOR_SAFE_THRESHOLD = 5.0.inches
 
 
-  val ELEVATOR_TOLERANCE = 0.0.inches
+  val ELEVATOR_TOLERANCE = 0.2.inches
 
-  val MAX_VELOCITY = 0.0.meters.perSecond
-  val MAX_ACCELERATION = 0.0.meters.perSecond.perSecond
+  val MAX_VELOCITY = 0.82.meters.perSecond
+  val MAX_ACCELERATION = 0.5.meters.perSecond.perSecond
 
   val SHOOT_SPEAKER_POSITION = 0.0.inches
   val SHOOT_AMP_POSITION = 0.0.inches
@@ -73,7 +73,7 @@ object ElevatorConstants {
 
   val VOLTAGE_COMPENSATION = 12.0.volts
   val ELEVATOR_PULLEY_TO_MOTOR = 4.0 / 1 * 4.0 / 1
-  val SPOOL_DIAMETER = 1.5.inches
+  val SPOOL_DIAMETER = 1.591.inches
 
   val LEADER_SUPPLY_CURRENT_LIMIT = 0.0.amps
   val LEADER_THRESHOLD_CURRENT_LIMIT = 0.0.amps

src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt

@@ -8,6 +8,7 @@ import com.team4099.robot2023.config.constants.ElevatorConstants
 import edu.wpi.first.wpilibj.RobotBase
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.Commands.runOnce
+import edu.wpi.first.wpilibj2.command.SubsystemBase
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.controller.ElevatorFeedforward
 import org.team4099.lib.controller.TrapezoidProfile
@@ -23,6 +24,7 @@ import org.team4099.lib.units.derived.inVolts
 import org.team4099.lib.units.derived.inVoltsPerInch
 import org.team4099.lib.units.derived.inVoltsPerInchPerSecond
 import org.team4099.lib.units.derived.inVoltsPerInchSeconds
+import org.team4099.lib.units.derived.inVoltsPerMeterPerSecondPerSecond
 import org.team4099.lib.units.derived.perInch
 import org.team4099.lib.units.derived.perInchSeconds
 import org.team4099.lib.units.derived.volts
@@ -31,7 +33,7 @@ import org.team4099.lib.units.perSecond
 import kotlin.time.Duration.Companion.seconds
 import com.team4099.robot2023.subsystems.superstructure.Request.ElevatorRequest as ElevatorRequest
 
-class Elevator(val io: ElevatorIO) {
+class Elevator(val io: ElevatorIO) : SubsystemBase() {
   val inputs = ElevatorIO.ElevatorInputs()
   private var elevatorFeedforward: ElevatorFeedforward =
     ElevatorFeedforward(
@@ -65,12 +67,13 @@ class Elevator(val io: ElevatorIO) {
     )
   private val kA =
     LoggedTunableValue(
-      "Elevator/kA", Pair({it.inVolts.perMetersPerSecondPerSecond}, {it.volts / 1.0.meters.perSecond.perSecond})
+      "Elevator/kA", Pair({it.inVoltsPerMeterPerSecondPerSecond}, {it.volts / 1.0.meters.perSecond.perSecond})
     )
 
   object TunableElevatorHeights {
     val enableElevator =
-      LoggedTunableNumber("Elevator/enableMovementElevator", ElevatorConstants.ENABLE_ELEVATOR)
+      LoggedTunableNumber("Elevator/enableMovementElevator",
+      if (ElevatorConstants.ENABLE_ELEVATOR) 1.0 else 0.0)
 
     val minPosition =
       LoggedTunableValue(
@@ -223,7 +226,7 @@ class Elevator(val io: ElevatorIO) {
 
   }
 
-  fun periodic() {
+  override fun periodic() {
     io.updateInputs(inputs)
     if ((kP.hasChanged()) || (kI.hasChanged()) || (kD.hasChanged())) {
       io.configPID(kP.get(), kI.get(), kD.get())
@@ -318,7 +321,7 @@ class Elevator(val io: ElevatorIO) {
               ElevatorConstants.HOMING_STALL_TIME_THRESHOLD
             )
         ) {
-          io.setVoltage(ElevatorConstants.HOMING_APPLIED_VOLTAGE)
+          io.setOutputVoltage(ElevatorConstants.HOMING_APPLIED_VOLTAGE)
         } else {
           zeroEncoder()
           isHomed = true
@@ -394,10 +397,10 @@ class Elevator(val io: ElevatorIO) {
   }
 
   fun elevatorClosedLoopRetractCommand(): Command {
-    return runOnce({ currentRequest = ElevatorRequest.TargetingPosition(12.inches) })
+    return runOnce({ currentRequest = ElevatorRequest.TargetingPosition(4.inches) })
   }
 
   fun testElevatorClosedLoopExtendCommand(): Command {
-    return runOnce({ currentRequest = ElevatorRequest.TargetingPosition(4.inches) })
+    return runOnce({ currentRequest = ElevatorRequest.TargetingPosition(16.inches) })
   }
 }

src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt

@@ -37,9 +37,6 @@ interface ElevatorIO {
     var leaderTempCelcius = 0.0.celsius
     var followerTempCelcius = 0.0.celsius
 
-    var leaderRawPosition = 0.0.rotations
-    var followerRawPosition = 0.0.rotations
-
     var isSimulating = false
 
     override fun toLog(table: LogTable) {
@@ -53,8 +50,6 @@ interface ElevatorIO {
       table?.put("elevatorFollowerSupplyCurrentAmps", followerSupplyCurrent.inAmperes)
       table?.put("elevatorLeaderTempCelsius", leaderTempCelcius.inCelsius)
       table?.put("elevatorFollowerTempCelsius", followerTempCelcius.inCelsius)
-      table?.put("elevatorLeaderRawPosition", leaderRawPosition)
-      table?.put("elevatorFollowRawPosition", followerRawPosition)
     }
 
     override fun fromLog(table: LogTable?) {
@@ -92,8 +87,6 @@ interface ElevatorIO {
       table?.get("elevatorFollowerTempCelcius", followerTempCelcius.inCelsius)?.let {
         followerTempCelcius = it.celsius
       }
-      table?.get("elevatorLeaderRawPosition", leaderRawPosition)?.let { leaderRawPosition = it }
-      table?.get("elevatorFollowerRawPosition", leaderRawPosition)?.let { followerRawPosition = it }
     }
   }
 

src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt

@@ -151,8 +151,6 @@ object ElevatorIOKraken : ElevatorIO {
     inputs.followerStatorCurrent = elevatorFollowerStatorCurrentSignal.value.amps
     inputs.leaderTempCelcius = elevatorLeadertempSignal.value.celsius
     inputs.followerTempCelcius = elevatorFollowertempSignal.value.celsius
-    inputs.leaderRawPosition = leaderSensor.getRawPosition()
-    inputs.followerRawPosition = followerSensor.getRawPosition()
   }
 
   override fun setOutputVoltage(voltage: ElectricalPotential) {

src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt

@@ -35,7 +35,7 @@ object ElevatorIONEO : ElevatorIO {
   private val leaderSensor =
     sparkMaxLinearMechanismSensor(
       leaderSparkMax,
-      ElevatorConstants.GEAR_RATIO,
+      ElevatorConstants.ELEVATOR_PULLEY_TO_MOTOR,
       ElevatorConstants.SPOOL_DIAMETER,
       ElevatorConstants.VOLTAGE_COMPENSATION
     )
@@ -46,7 +46,7 @@ object ElevatorIONEO : ElevatorIO {
   private val followerSensor =
     sparkMaxLinearMechanismSensor(
       followerSparkMax,
-      ElevatorConstants.GEAR_RATIO,
+      ElevatorConstants.ELEVATOR_PULLEY_TO_MOTOR,
       ElevatorConstants.SPOOL_DIAMETER,
       ElevatorConstants.VOLTAGE_COMPENSATION
     )
@@ -132,10 +132,6 @@ object ElevatorIONEO : ElevatorIO {
 
     inputs.followerTempCelcius = followerSparkMax.motorTemperature.celsius
 
-    inputs.leaderRawPosition = leaderSparkMax.encoder.position
-
-    inputs.followerRawPosition = followerSparkMax.encoder.position
-
     Logger.recordOutput("Elevator/leaderRawRotations", leaderSparkMax.encoder.position)
   }
 

src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt

@@ -10,6 +10,7 @@ import edu.wpi.first.math.system.plant.DCMotor
 import edu.wpi.first.wpilibj.simulation.BatterySim
 import edu.wpi.first.wpilibj.simulation.ElevatorSim
 import edu.wpi.first.wpilibj.simulation.RoboRioSim
+import org.littletonrobotics.junction.Logger
 import org.team4099.lib.controller.PIDController
 import org.team4099.lib.units.base.Length
 import org.team4099.lib.units.base.Meter
@@ -34,7 +35,7 @@ object ElevatorIOSim : ElevatorIO {
   val elevatorSim: ElevatorSim =
     ElevatorSim(
       DCMotor.getNEO(2),
-      ElevatorConstants.GEAR_RATIO,
+      ElevatorConstants.ELEVATOR_PULLEY_TO_MOTOR,
       ElevatorConstants.CARRIAGE_MASS.inKilograms,
       ElevatorConstants.SPOOL_DIAMETER.inMeters / 2,
       ElevatorConstants.ELEVATOR_MAX_RETRACTION.inMeters,
@@ -83,9 +84,6 @@ object ElevatorIOSim : ElevatorIO {
     inputs.followerSupplyCurrent = elevatorSim.currentDrawAmps.amps / 2
     inputs.followerAppliedVoltage = lastAppliedVoltage
 
-    inputs.leaderRawPosition = 0.0
-    inputs.followerRawPosition = 0.0
-
     inputs.isSimulating = true
 
     RoboRioSim.setVInVoltage(
@@ -100,9 +98,7 @@ object ElevatorIOSim : ElevatorIO {
    * @param voltage the voltage to set the motor to
    */
   override fun setOutputVoltage(voltage: ElectricalPotential) {
-    if (!((elevatorSim.positionMeters.meters < 0.5.inches) && (voltage < 0.volts)) &&
-      !(elevatorSim.positionMeters.meters > ElevatorConstants.ELEVATOR_MAX_EXTENSION - 0.5.inches && (voltage > 0.volts))
-    ) {
+    Logger.recordOutput("Elevator/OutputTest", voltage)
       val clampedVoltage =
         clamp(
           voltage,
@@ -112,7 +108,6 @@ object ElevatorIOSim : ElevatorIO {
       lastAppliedVoltage = clampedVoltage
 
       elevatorSim.setInputVoltage(clampedVoltage.inVolts)
-    }
   }
 
   /**

vendordeps/PathplannerLib.json

@@ -1,7 +1,7 @@
 {
     "fileName": "PathplannerLib.json",
     "name": "PathplannerLib",
-    "version": "2024.0.0-beta-6",
+    "version": "2024.1.4",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "frcYear": "2024",
     "mavenUrls": [
@@ -12,15 +12,15 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2024.0.0-beta-6"
+            "version": "2024.1.4"
         }
     ],
     "jniDependencies": [],
     "cppDependencies": [
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2024.0.0-beta-6",
+            "version": "2024.1.4",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,