auto score and operator app

team4099 · Oct 21, 2023 · 995d367 · 995d367
1 parent a1973e9
commit 995d367
Show file tree

Hide file tree

Showing 6 changed files with 63 additions and 17 deletions.
diff --git a/src/main/deploy/nodeselector/index.html b/src/main/deploy/nodeselector/index.html
@@ -123,6 +123,17 @@
           <td class="node cube"></td>
           <td class="node cone"></td>
         </tr>
+        <tr style="height: 60px; font-size: 40px; text-align: center; font-family: mono; font-weight: 700;">
+          <td class="">1</td>
+          <td class="">2</td>
+          <td class="">3</td>
+          <td class="">4</td>
+          <td class="">5</td>
+          <td class="">6</td>
+          <td class="">7</td>
+          <td class="">8</td>
+          <td class="">9</td>
+        </tr>
       </tbody>
     </table>
 

diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -78,9 +78,9 @@ object RobotContainer {
         )
       superstructure =
         Superstructure(
-          Elevator(ElevatorIONeo),
-          GroundIntake(GroundIntakeIONeo),
-          Manipulator(ManipulatorIONeo),
+          Elevator(object: ElevatorIO {}),
+          GroundIntake(object: GroundIntakeIO {}),
+          Manipulator(object: ManipulatorIO {}),
           Led(object : LedIO {}),
           GameBoy(GameboyIOServer)
         )

diff --git a/src/main/kotlin/com/team4099/robot2023/commands/AutoScoreCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/AutoScoreCommand.kt
@@ -15,11 +15,14 @@ import com.team4099.robot2023.subsystems.gameboy.objective.isConeNode
 import com.team4099.robot2023.subsystems.superstructure.Superstructure
 import com.team4099.robot2023.util.AllianceFlipUtil
 import com.team4099.robot2023.util.FMSData
+import com.team4099.robot2023.util.Velocity2d
 import edu.wpi.first.math.geometry.Rotation2d
 import edu.wpi.first.wpilibj2.command.Commands.runOnce
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.geometry.Pose2d
+import org.team4099.lib.geometry.Pose3d
+import org.team4099.lib.units.base.inches
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.degrees
@@ -30,6 +33,7 @@ import org.team4099.lib.units.perSecond
 class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstructure) :
   SequentialCommandGroup() {
   lateinit var drivePose: Pose2d
+  lateinit var finalDrivePose: Pose2d
   lateinit var finalPose: Pose2d
   lateinit var postAlignPose: Pose2d
   lateinit var intermediaryWaypoints: List<Waypoint>
@@ -61,7 +65,7 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru
         finalPose =
           AllianceFlipUtil.apply(
             Pose2d(
-              2.16.meters, // slightly offset in the x
+              2.00.meters, // slightly offset in the x
               FieldConstants.Grids.nodeFirstY +
                 FieldConstants.Grids.nodeSeparationY *
                 (if (FMSData.isBlue) superstructure.objective.nodeColumn
@@ -99,37 +103,63 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru
         drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(false) }
       })
 
+    val updatedDrivePose = runOnce({
+      finalDrivePose = drivetrain.odometryPose
+    })
+
     addCommands(
       setupCommand,
       DrivePathCommand(
         drivetrain,
         {
           listOf(
             Waypoint(
-              drivePose.pose2d.translation,
+              drivetrain.odometryPose.pose2d.translation,
               if (drivetrain.fieldVelocity.magnitude.absoluteValue < 0.25.meters.perSecond)
                 null
               else heading.inRotation2ds,
-              drivePose.rotation.inRotation2ds
+              drivetrain.odometryPose.rotation.inRotation2ds
             )
           ) +
-            intermediaryWaypoints +
+            //intermediaryWaypoints +
             listOf(
               Waypoint(
                 finalPose.translation.translation2d,
+                null,
                 finalPose.rotation.inRotation2ds
-              ),
+              )
+            )
+        },
+        keepTrapping = false,
+        flipForAlliances = false,
+        endVelocity= Velocity2d(0.meters.perSecond, 0.0.meters.perSecond),
+        tolerance = Pose2d(5.inches, 3.inches, 3.degrees),
+        forceRobotVelocityCheck=true
+      ),
+      updatedDrivePose,
+      DrivePathCommand(
+        drivetrain,
+        {
+          listOf(
+            Waypoint(
+              finalDrivePose.pose2d.translation,
+              null,
+              finalDrivePose.rotation.inRotation2ds
+            )
+          ) +
+            //intermediaryWaypoints +
+            listOf(
               Waypoint(
                 postAlignPose.translation.translation2d,
+                null,
                 postAlignPose.rotation.inRotation2ds
               )
             )
         },
-        keepTrapping = true,
+        keepTrapping = false,
         flipForAlliances = false
       ),
-      superstructure.prepScoreCommand({ gamePiece }, { nodeTier }),
-      superstructure.score()
+      superstructure.prepScoreCommand({ gamePiece }, { nodeTier })
       //      ParallelCommandGroup(
       //        superstructure.prepScoreCommand({ gamePiece }, { nodeTier }),
       //        DrivePathCommand(

diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
@@ -64,6 +64,8 @@ class DrivePathCommand(
   val endPathOnceAtReference: Boolean = true,
   val leaveOutYAdjustment: Boolean = false,
   val endVelocity: Velocity2d = Velocity2d(),
+  val tolerance: Pose2d = Pose2d(1.inches, 1.inches, 1.degrees),
+  val forceRobotVelocityCheck: Boolean = false
 ) : CommandBase() {
   private val xPID: PIDController<Meter, Velocity<Meter>>
   private val yPID: PIDController<Meter, Velocity<Meter>>
@@ -198,7 +200,7 @@ class DrivePathCommand(
         xPID.wpiPidController, yPID.wpiPidController, thetaPID.wpiPidController
       )
 
-    swerveDriveController.setTolerance(Pose2d(3.inches, 3.inches, 3.degrees).pose2d)
+    swerveDriveController.setTolerance(tolerance.pose2d)
   }
 
   override fun initialize() {
@@ -331,11 +333,14 @@ class DrivePathCommand(
     trajCurTime = Clock.fpgaTime - trajStartTime
     return endPathOnceAtReference &&
       (!keepTrapping || swerveDriveController.atReference()) &&
-      trajCurTime > trajectoryGenerator.driveTrajectory.totalTimeSeconds.seconds
+      trajCurTime > trajectoryGenerator.driveTrajectory.totalTimeSeconds.seconds && (
+        drivetrain.fieldVelocity.magnitude < 0.1.meters.perSecond || !forceRobotVelocityCheck
+        )
   }
 
   override fun end(interrupted: Boolean) {
     Logger.getInstance().recordOutput("ActiveCommands/DrivePathCommand", false)
+    drivetrain.setOpenLoop(0.degrees.perSecond, Pair(0.meters.perSecond, 0.meters.perSecond))
     if (interrupted) {
       // Stop where we are if interrupted
       drivetrain.setOpenLoop(0.degrees.perSecond, Pair(0.meters.perSecond, 0.meters.perSecond))

diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt
@@ -54,8 +54,8 @@ object DrivetrainConstants {
   val SLOW_AUTO_VEL = 2.meters.perSecond
   val SLOW_AUTO_ACCEL = 2.0.meters.perSecond.perSecond
 
-  val MAX_AUTO_VEL = 2.meters.perSecond // 4
-  val MAX_AUTO_ACCEL = 1.meters.perSecond.perSecond // 3
+  val MAX_AUTO_VEL = 3.meters.perSecond // 4
+  val MAX_AUTO_ACCEL = 3.5.meters.perSecond.perSecond // 3
 
   val MAX_AUTO_BRAKE_VEL = 0.5.meters.perSecond // 4
   val MAX_AUTO_BRAKE_ACCEL = 0.5.meters.perSecond.perSecond // 3
@@ -92,7 +92,7 @@ object DrivetrainConstants {
     val AUTO_POS_KPX: ProportionalGain<Meter, Velocity<Meter>>
       get() {
         if (RobotBase.isReal()) {
-          return 0.meters.perSecond / 1.0.meters // todo:4
+          return 0.25.meters.perSecond / 1.0.meters // todo:4
         } else {
           return 7.0.meters.perSecond / 1.0.meters
         }

diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/GroundIntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/GroundIntakeConstants.kt
@@ -42,7 +42,7 @@ object GroundIntakeConstants {
   const val ROLLER_MOTOR_INVERTED = true
   const val ARM_MOTOR_INVERTED = false
 
-  val ABSOLUTE_ENCODER_OFFSET = (-159.87).degrees
+  val ABSOLUTE_ENCODER_OFFSET = (-165.87).degrees
   // From encoder to intake
   val ROLLER_GEAR_RATIO = (36.0.driven / 18.0.driving).gearRatio