Parallel deadline in auto, no end condition for elevator moveToPos, u…

…pdate constants,
blair-robot-project · Dec 5, 2023 · 39727c7 · 39727c7
1 parent 3d72d2b
commit 39727c7
Show file tree

Hide file tree

Showing 15 changed files with 118 additions and 90 deletions.
diff --git a/simgui-ds.json b/simgui-ds.json
@@ -94,8 +94,7 @@
       "guid": "Keyboard0"
     },
     {
-      "guid": "78696e70757401000000000000000000",
-      "useGamepad": true
+      "guid": "Keyboard1"
     }
   ]
 }
diff --git a/simgui.json b/simgui.json
@@ -49,12 +49,8 @@
         }
       },
       "/SmartDashboard/Field": {
-        "bottom": 544,
-        "builtin": "2023 Charged Up",
         "height": 8.013679504394531,
-        "left": 46,
-        "right": 1088,
-        "top": 36,
+        "image": "C:\\Users\\jpoth\\Downloads\\field(1).png",
         "width": 16.541748046875,
         "window": {
           "visible": true
@@ -79,18 +75,20 @@
         "open": true,
         "robot": {
           "drive": {
+            "double[]##v_/Monologuing/robot/drive/1.1 Estimated Pose": {
+              "open": true
+            },
             "double[]##v_/Monologuing/robot/drive/Current Chassis Speeds": {
               "open": true
             },
             "double[]##v_/Monologuing/robot/drive/Estimated Pose": {
               "open": true
             }
           },
-          "elevator": {
-            "open": true
-          },
           "intake": {
-            "open": true
+            "double[]##v_/Monologuing/robot/intake/3.1 Intake 3D Position": {
+              "open": true
+            }
           },
           "open": true
         }

diff --git a/src/main/kotlin/frc/team449/RobotLoop.kt b/src/main/kotlin/frc/team449/RobotLoop.kt
@@ -1,9 +1,7 @@
 package frc.team449
 
-import edu.wpi.first.wpilibj.DriverStation
+import edu.wpi.first.wpilibj.*
 import edu.wpi.first.wpilibj.RobotBase
-import edu.wpi.first.wpilibj.TimedRobot
-import edu.wpi.first.wpilibj.Timer
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.CommandScheduler
@@ -45,7 +43,7 @@ class RobotLoop : TimedRobot(), Logged {
     SmartDashboard.putData("Field", robot.field)
     SmartDashboard.putData("Routine Chooser", routineChooser)
     SmartDashboard.putData("Command Scheduler", CommandScheduler.getInstance())
-    SmartDashboard.putBoolean("Enable Logging?", true)
+    SmartDashboard.putBoolean("Enable Logging?", false)
 
     controllerBinder.bindButtons()
 
@@ -63,8 +61,10 @@ class RobotLoop : TimedRobot(), Logged {
 
     robot.field.getObject("bumpers").pose = robot.drive.pose
 
-    if (SmartDashboard.getBoolean("Enable Logging?", true)) {
+    if (SmartDashboard.getBoolean("Enable Logging?", false)) {
       Monologue.update()
+    } else {
+      Monologue.updateFileLog()
     }
   }
 

diff --git a/src/main/kotlin/frc/team449/control/TrapezoidalExponentialProfile.kt b/src/main/kotlin/frc/team449/control/TrapezoidalExponentialProfile.kt
@@ -1,6 +1,7 @@
 package frc.team449.control
 
 import edu.wpi.first.math.util.Units
+import frc.team449.robot2023.constants.MotorConstants
 import frc.team449.robot2023.constants.subsystem.ElevatorConstants
 import kotlin.math.*
 
@@ -32,10 +33,10 @@ class TrapezoidalExponentialProfile(
   }
 
   // NEO Motor Constants
-  val freeSpeed = ElevatorConstants.FREE_SPEED
-  val freeCurrent = ElevatorConstants.FREE_CURRENT
-  val stallCurrent = ElevatorConstants.STALL_CURRENT
-  val stallTorque = ElevatorConstants.STALL_TORQUE
+  val freeSpeed = MotorConstants.FREE_SPEED
+  val freeCurrent = MotorConstants.FREE_CURRENT
+  val stallCurrent = MotorConstants.STALL_CURRENT
+  val stallTorque = MotorConstants.STALL_TORQUE
 
   private fun expDecelIntercept(
     vFree: Double,

diff --git a/src/main/kotlin/frc/team449/control/auto/ChoreoRoutine.kt b/src/main/kotlin/frc/team449/control/auto/ChoreoRoutine.kt
@@ -44,7 +44,7 @@ class ChoreoRoutine(
 
     for (i in 0 until trajectories.size) {
       ezraGallun.addCommands(
-        ParallelCommandGroup(
+        ParallelDeadlineGroup(
           ChoreoFollower(
             drive,
             trajectories[i],

diff --git a/src/main/kotlin/frc/team449/robot2023/auto/routines/OnePiecePick.kt b/src/main/kotlin/frc/team449/robot2023/auto/routines/OnePiecePick.kt
@@ -17,7 +17,8 @@ class OnePiecePick(
     ChoreoRoutine(
       drive = robot.drive,
       parallelEventMap = hashMapOf(
-        0 to robot.elevator.high(),
+        0 to robot.intake.extend().andThen(
+          robot.elevator.high()),
         1 to robot.elevator.stow()
       ),
       stopEventMap = hashMapOf(

diff --git a/src/main/kotlin/frc/team449/robot2023/constants/MotorConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/MotorConstants.kt
@@ -0,0 +1,13 @@
+package frc.team449.robot2023.constants
+
+import edu.wpi.first.math.util.Units
+
+object MotorConstants {
+  /** NEO characteristics */
+  const val EFFICIENCY = 0.90
+  const val NOMINAL_VOLTAGE = 12.0
+  const val STALL_TORQUE = 3.36 * EFFICIENCY
+  const val STALL_CURRENT = 166.0
+  const val FREE_CURRENT = 1.3
+  val FREE_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(5676.0)
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/constants/RobotConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/RobotConstants.kt
@@ -3,6 +3,7 @@ package frc.team449.robot2023.constants
 import edu.wpi.first.math.controller.PIDController
 import edu.wpi.first.math.geometry.Pose2d
 import edu.wpi.first.math.geometry.Rotation2d
+import edu.wpi.first.math.system.plant.DCMotor
 import edu.wpi.first.math.util.Units
 import edu.wpi.first.wpilibj.RobotBase
 import frc.team449.robot2023.constants.drives.SwerveConstants
@@ -18,10 +19,21 @@ object RobotConstants {
   const val TRANSLATION_DEADBAND = .15
   const val ROTATION_DEADBAND = .15
 
+  /** In kilograms, include bumpers and battery and all */
+  const val ROBOT_WEIGHT = 60
+
   /** Drive configuration */
   const val MAX_LINEAR_SPEED = SwerveConstants.MAX_ATTAINABLE_MK4I_SPEED // m/s
   const val MAX_ROT_SPEED = PI // rad/s
-  val MAX_ACCEL = if (RobotBase.isSimulation()) 7.5 else 14.75 // m/s/s
+  val MAX_ACCEL = if (RobotBase.isSimulation()) 12.2625 else
+    4 * DCMotor(
+      MotorConstants.NOMINAL_VOLTAGE,
+      MotorConstants.STALL_TORQUE,
+      MotorConstants.STALL_CURRENT,
+      MotorConstants.FREE_CURRENT,
+      MotorConstants.FREE_SPEED,
+      1
+    ).getTorque(80.0) * SwerveConstants.DRIVE_GEARING / (Units.inchesToMeters(2.0) * ROBOT_WEIGHT) // m/s/s
   val INITIAL_POSE = Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0))
 
   const val LOOP_TIME = 0.020

diff --git a/src/main/kotlin/frc/team449/robot2023/constants/subsystem/ElevatorConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/subsystem/ElevatorConstants.kt
@@ -8,33 +8,25 @@ import kotlin.math.PI
 
 object ElevatorConstants {
 
-  /** NEO characteristics */
-  const val EFFICIENCY = 0.90
-  const val NOMINAL_VOLTAGE = 12.0
-  const val STALL_TORQUE = 3.36 * EFFICIENCY
-  const val STALL_CURRENT = 166.0
-  const val FREE_CURRENT = 1.3
-  val FREE_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(5676.0)
-
   /** Mechanism2d Visual constants */
   const val ANGLE = 75.0
   const val MIN_LENGTH = 0.1
   const val WIDTH = 10.0
   val COLOR = Color8Bit(255, 0, 255)
   val DESIRED_COLOR = Color8Bit(0, 255, 0)
 
-  const val LEFT_ID = 20
+  const val LEFT_ID = 60
   const val LEFT_INVERTED = false
-  const val RIGHT_ID = 21
+  const val RIGHT_ID = 61
   const val RIGHT_INVERTED = true
   const val EFFECTIVE_GEARING = 6.48 / 2
   const val PULLEY_RADIUS = 0.022352
   const val UPR = PULLEY_RADIUS * 2 * PI
 
   var kS = 0.0
-  var kG = 1.075
+  var kG = 1.26725
 
-  const val HIGH_DISTANCE = 1.4
+  const val HIGH_DISTANCE = 1.114425
   const val LOW_DISTANCE = 0.25
   const val STOW_DISTANCE = 0.0
 

diff --git a/src/main/kotlin/frc/team449/robot2023/constants/subsystem/IntakeConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/subsystem/IntakeConstants.kt
@@ -2,11 +2,13 @@ package frc.team449.robot2023.constants.subsystem
 
 object IntakeConstants {
 
-  const val MOTOR_ID = 25
+  const val MOTOR_ID = 62
   const val INVERTED = false
 
   const val FORWARD = 0
   const val REVERSE = 1
 
   const val INTAKE_VOLTAGE = 4.0
+
+  const val EXTENSION_TIME = 0.25
 }
diff --git a/src/main/kotlin/frc/team449/robot2023/constants/subsystem/ManipulatorConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/subsystem/ManipulatorConstants.kt
@@ -2,7 +2,7 @@ package frc.team449.robot2023.constants.subsystem
 
 object ManipulatorConstants {
 
-  const val MOTOR_ID = 26
+  const val MOTOR_ID = 59
   const val INVERTED = false
 
   val INTAKE_VOLTAGE = 3.0

diff --git a/src/main/kotlin/frc/team449/robot2023/subsystems/ControllerBindings.kt b/src/main/kotlin/frc/team449/robot2023/subsystems/ControllerBindings.kt
@@ -16,20 +16,31 @@ class ControllerBindings(
 
   fun bindButtons() {
     JoystickButton(mechanismController, XboxController.Button.kX.value).onTrue(
-      robot.elevator.low()
+      SequentialCommandGroup(
+        // How long does it take to extend and retract the intake? Is it the play to have mech manually extend intake right as we enter the
+        // community or have it automated such that it extends intake and moves elevator at the same time?
+//        robot.intake.extend(),
+        robot.elevator.low()
+      )
     )
 
     JoystickButton(mechanismController, XboxController.Button.kY.value).onTrue(
-      robot.elevator.high()
+      SequentialCommandGroup(
+//        robot.intake.extend(),
+        robot.elevator.high()
+      )
     )
 
     JoystickButton(mechanismController, XboxController.Button.kB.value).onTrue(
-      robot.elevator.stow()
+      SequentialCommandGroup(
+//        robot.intake.extend(),
+        robot.elevator.stow()
+      )
     )
 
     JoystickButton(mechanismController, XboxController.Button.kA.value).onTrue(
       SequentialCommandGroup(
-        robot.elevator.showSummaryStats(),
+        robot.elevator.summaryStats(),
         robot.elevator.tuneKG()
       )
     )
@@ -55,7 +66,9 @@ class ControllerBindings(
     )
 
     Trigger { abs(mechanismController.leftY) > 0.25 }.onTrue(
-      robot.elevator.manualMovement({ mechanismController.leftY })
+      robot.elevator.manualMovement({ -mechanismController.leftY })
+    ).onFalse(
+      robot.elevator.defaultCommand
     )
 
     Trigger { driveController.leftTriggerAxis > 0.8 }.onTrue(

diff --git a/src/main/kotlin/frc/team449/robot2023/subsystems/Intake.kt b/src/main/kotlin/frc/team449/robot2023/subsystems/Intake.kt
@@ -7,7 +7,9 @@ import edu.wpi.first.wpilibj.DoubleSolenoid
 import edu.wpi.first.wpilibj.PneumaticsModuleType
 import edu.wpi.first.wpilibj.RobotBase
 import edu.wpi.first.wpilibj2.command.Command
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
 import edu.wpi.first.wpilibj2.command.SubsystemBase
+import edu.wpi.first.wpilibj2.command.WaitCommand
 import frc.team449.robot2023.constants.subsystem.IntakeConstants
 import frc.team449.system.SparkUtil
 
@@ -32,7 +34,12 @@ class Intake(
   }
 
   fun extend(): Command {
-    return this.runOnce { piston.set(DoubleSolenoid.Value.kForward) }
+    return SequentialCommandGroup(
+      this.runOnce {
+        piston.set(DoubleSolenoid.Value.kForward)
+      },
+      WaitCommand(IntakeConstants.EXTENSION_TIME)
+    )
   }
 
   fun retract(): Command {
@@ -62,7 +69,7 @@ class Intake(
 
   override fun initSendable(builder: SendableBuilder) {
     builder.publishConstString("1.0", "Piston")
-    builder.addStringProperty("Piston Status", { piston.get().toString() }, {})
+    builder.addStringProperty("1.1 Piston Status", { piston.get().toString() }, {})
 
     builder.publishConstString("2.0", "Motor Voltages")
     builder.addDoubleProperty("2.1 Last Voltage", { lastVoltage }, null)