climb auto setpoint, log ir sensor, swerve turn enc, pivot move to an…

…gle slowly
blair-robot-project · Mar 2, 2024 · d236eaa · d236eaa
1 parent 17b489b
commit d236eaa
Show file tree

Hide file tree

Showing 6 changed files with 34 additions and 7 deletions.
diff --git a/simgui-ds.json b/simgui-ds.json
@@ -90,7 +90,6 @@
     }
   ],
   "robotJoysticks": [
-    {},
     {
       "guid": "Keyboard0"
     }

diff --git a/src/main/kotlin/frc/team449/robot2024/Robot.kt b/src/main/kotlin/frc/team449/robot2024/Robot.kt
@@ -57,6 +57,9 @@ class Robot : RobotBase(), Logged {
   @Log.NT
   val climber = createClimber(this)
 
+  @Log.NT
   val infrared = DigitalInput(RobotConstants.IR_CHANNEL)
+
+  @Log.NT
   val closeToShooterInfrared = DigitalInput(RobotConstants.IR_CHANNEL2)
 }
diff --git a/src/main/kotlin/frc/team449/robot2024/constants/drives/SwerveConstants.kt b/src/main/kotlin/frc/team449/robot2024/constants/drives/SwerveConstants.kt
@@ -17,7 +17,7 @@ object SwerveConstants {
 
   /** Turning encoder channels */
   const val TURN_ENC_CHAN_FL = 5
-  const val TURN_ENC_CHAN_FR = 6
+  const val TURN_ENC_CHAN_FR = 4
   const val TURN_ENC_CHAN_BL = 7
   const val TURN_ENC_CHAN_BR = 8
 

diff --git a/src/main/kotlin/frc/team449/robot2024/constants/subsystem/PivotConstants.kt b/src/main/kotlin/frc/team449/robot2024/constants/subsystem/PivotConstants.kt
@@ -56,10 +56,12 @@ object PivotConstants {
 
   // Max at 40A should be 10.567679154992222
   const val MAX_ACCEL = 8.0
+  const val SLOW_ACCEL = 3.0
 
   val MIN_ANGLE = Units.degreesToRadians(0.0)
   val MAX_ANGLE = Units.degreesToRadians(105.0)
   val AMP_ANGLE = Units.degreesToRadians(100.53)
+  val CLIMB_ANGLE = Units.degreesToRadians(75.0)
   val STOW_ANGLE = Units.degreesToRadians(-1.0)
 
   // IS THIS CORRECT???

diff --git a/src/main/kotlin/frc/team449/robot2024/subsystems/ControllerBindings.kt b/src/main/kotlin/frc/team449/robot2024/subsystems/ControllerBindings.kt
@@ -134,10 +134,14 @@ class ControllerBindings(
 
     robot.mechController.povUp().onTrue(
       robot.pivot.manualUp()
+    ).onFalse(
+      robot.pivot.hold()
     )
 
     robot.mechController.povDown().onTrue(
       robot.pivot.manualDown()
+    ).onFalse(
+      robot.pivot.hold()
     )
 
     mechanismController.a().onTrue(
@@ -191,10 +195,8 @@ class ControllerBindings(
       )
     )
 
-    mechanismController.start().onTrue(
-      robot.feeder.autoShootIntake()
-    ).onFalse(
-      robot.feeder.stop()
+    mechanismController.x().onTrue(
+      robot.pivot.moveClimb()
     )
 
     mechanismController.back().onTrue(
@@ -205,7 +207,7 @@ class ControllerBindings(
         .withTimeout(FeederConstants.CHECK_NOTE_IN_LOCATION_TIMEOUT_SECONDS)
     )
 
-    mechanismController.x().onTrue(
+    mechanismController.start().onTrue(
       ParallelCommandGroup(
         robot.feeder.outtake(),
         robot.shooter.duringIntake()

diff --git a/src/main/kotlin/frc/team449/robot2024/subsystems/pivot/Pivot.kt b/src/main/kotlin/frc/team449/robot2024/subsystems/pivot/Pivot.kt
@@ -40,6 +40,13 @@ open class Pivot(
   private val robot: Robot
 ) : SubsystemBase() {
 
+  private val slowProfile = TrapezoidProfile(
+    TrapezoidProfile.Constraints(
+      PivotConstants.MAX_VELOCITY,
+      PivotConstants.SLOW_ACCEL
+    )
+  )
+
   open val positionSupplier: Supplier<Double> =
     Supplier { encoder.position }
 
@@ -126,6 +133,12 @@ open class Pivot(
     }
   }
 
+  fun moveClimb(): Command {
+    return this.run {
+      moveToAngleSlow(PivotConstants.CLIMB_ANGLE)
+    }
+  }
+
   fun autoAngle(): Command {
     return this.run {
       moveToAngle(PivotConstants.AUTO_ANGLE)
@@ -194,6 +207,14 @@ open class Pivot(
     }
   }
 
+  private fun moveToAngleSlow(goal: Double) {
+    lastProfileReference = slowProfile.calculate(RobotConstants.LOOP_TIME, lastProfileReference, TrapezoidProfile.State(goal, 0.0))
+
+    correctAndPredict()
+
+    motor.setVoltage(getVoltage())
+  }
+
   private fun moveToAngle(goal: Double) {
     lastProfileReference = profile.calculate(RobotConstants.LOOP_TIME, lastProfileReference, TrapezoidProfile.State(goal, 0.0))