code so far

team4099 · Feb 19, 2024 · 0be0a4e · 0be0a4e
1 parent b581e37
commit 0be0a4e
Show file tree

Hide file tree

Showing 8 changed files with 71 additions and 181 deletions.
diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
@@ -4,9 +4,9 @@
   "holonomicMode": true,
   "pathFolders": [],
   "autoFolders": [],
-  "defaultMaxVel": 3.0,
-  "defaultMaxAccel": 3.0,
-  "defaultMaxAngVel": 270.0,
-  "defaultMaxAngAccel": 600.0,
-  "maxModuleSpeed": 3.0
+  "defaultMaxVel": 1.0,
+  "defaultMaxAccel": 1.0,
+  "defaultMaxAngVel": 151.0,
+  "defaultMaxAngAccel": 302.0,
+  "maxModuleSpeed": 4.26
 }
diff --git a/src/main/deploy/pathplanner/paths/5NotePureMove.path b/src/main/deploy/pathplanner/paths/5NotePureMove.path
@@ -3,137 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.2289055048108568,
-        "y": 5.4387004560018815
+        "x": 1.332914114902304,
+        "y": 5.568829433955164
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.7231721740590975,
-        "y": 3.908136928915673
+        "x": 2.1608123307389997,
+        "y": 5.568829433955164
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.22637826150855,
-        "y": 4.12372243302023
+        "x": 3.685248097348047,
+        "y": 5.568829433955164
       },
       "prevControl": {
-        "x": 2.172429912120765,
-        "y": 4.09804759236747
-      },
-      "nextControl": {
-        "x": 2.280326610896335,
-        "y": 4.1493972736729905
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 1.371676329252173,
-        "y": 5.4387004560018815
-      },
-      "prevControl": {
-        "x": 1.1350935870429284,
-        "y": 5.125820818897388
-      },
-      "nextControl": {
-        "x": 1.6082590714614176,
-        "y": 5.751580093106375
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 2.6622268722657387,
-        "y": 5.462296362194021
-      },
-      "prevControl": {
-        "x": 2.07483477608705,
-        "y": 5.411457747949859
-      },
-      "nextControl": {
-        "x": 2.7066640964687334,
-        "y": 5.466142390960894
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 1.1878985224745173,
-        "y": 5.696696223214165
-      },
-      "prevControl": {
-        "x": 2.129863497405182,
-        "y": 5.953154353689664
-      },
-      "nextControl": {
-        "x": 0.966076214972528,
-        "y": 5.636303176982367
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 2.4021521705928346,
-        "y": 6.677901317715116
-      },
-      "prevControl": {
-        "x": 1.8381221065820927,
-        "y": 5.508856306149623
-      },
-      "nextControl": {
-        "x": 2.417172472108316,
-        "y": 6.709033361340628
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 1.0688795243068614,
-        "y": 5.985668113961836
-      },
-      "prevControl": {
-        "x": 2.1442753447778573,
-        "y": 6.4137426928772
-      },
-      "nextControl": {
-        "x": 0.9454947637610032,
-        "y": 5.936553287207062
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 7.766426772723887,
-        "y": 7.256520752911847
-      },
-      "prevControl": {
-        "x": 3.670312735497631,
-        "y": 6.7083552256964465
-      },
-      "nextControl": {
-        "x": 8.420823369195706,
-        "y": 7.344095867289241
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 1.654099576432541,
-        "y": 6.019814612562775
-      },
-      "prevControl": {
-        "x": 5.070880344350176,
-        "y": 6.220795602050137
+        "x": 2.255248097348047,
+        "y": 5.568829433955164
       },
       "nextControl": null,
       "isLocked": false,
@@ -142,22 +30,12 @@
   ],
   "rotationTargets": [
     {
-      "waypointRelativePos": 0,
+      "waypointRelativePos": 1,
       "rotationDegrees": 0,
       "rotateFast": false
     },
     {
-      "waypointRelativePos": 5,
-      "rotationDegrees": 28.75499731851916,
-      "rotateFast": false
-    },
-    {
-      "waypointRelativePos": 7,
-      "rotationDegrees": 14.598436530771252,
-      "rotateFast": false
-    },
-    {
-      "waypointRelativePos": 3,
+      "waypointRelativePos": 0,
       "rotationDegrees": 0,
       "rotateFast": false
     }
@@ -167,16 +45,19 @@
   "globalConstraints": {
     "maxVelocity": 1.0,
     "maxAcceleration": 1.0,
-    "maxAngularVelocity": 270.0,
-    "maxAngularAcceleration": 500.0
+    "maxAngularVelocity": 151.0,
+    "maxAngularAcceleration": 302.0
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 22.463820093818477,
+    "rotation": 0.8468178483161829,
     "rotateFast": false
   },
   "reversed": false,
   "folder": null,
-  "previewStartingState": null,
+  "previewStartingState": {
+    "rotation": -0.3056623717569552,
+    "velocity": 0
+  },
   "useDefaultConstraints": false
 }
diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
@@ -32,10 +32,10 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 270.0,
-    "maxAngularAcceleration": 600.0
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 151.0,
+    "maxAngularAcceleration": 302.0
   },
   "goalEndState": {
     "velocity": 0,

diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -28,6 +28,7 @@ import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO
 import com.team4099.robot2023.subsystems.superstructure.Request
 import com.team4099.robot2023.subsystems.superstructure.Superstructure
 import com.team4099.robot2023.subsystems.vision.Vision
+import com.team4099.robot2023.subsystems.vision.camera.CameraIO
 import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar
 import com.team4099.robot2023.subsystems.wrist.Wrist
 import com.team4099.robot2023.subsystems.wrist.WristIO
@@ -77,9 +78,9 @@ object RobotContainer {
       drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim)
       vision =
         Vision(
-          CameraIONorthstar("northstar_1"),
-          CameraIONorthstar("northstar_2"),
-          CameraIONorthstar("northstar_3"),
+          object: CameraIO {},
+          object: CameraIO {},
+          object: CameraIO {},
         )
       limelight = LimelightVision(object : LimelightVisionIO {})
       intake = Intake(IntakeIOSim)
@@ -152,17 +153,17 @@ object RobotContainer {
   fun mapTeleopControls() {
 
     ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 0.degrees))
-    // ControlBoard.runGroundIntake.whileTrue(superstructure.groundIntakeCommand())
-    //    ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand())
-    //    ControlBoard.prepAmpScore.whileTrue(superstructure.prepAmpCommand())
-    //    ControlBoard.ampScore.whileTrue(superstructure.scoreAmpCommand())
-    //    ControlBoard.scoreSpeakerLow.whileTrue(superstructure.scoreSpeakerLowCommand())
-    //    ControlBoard.scoreSpeakerMid.whileTrue(superstructure.scoreSpeakerMidCommand())
-    //    ControlBoard.scoreSpeakerHigh.whileTrue(superstructure.scoreSpeakerHighCommand())
-    //    ControlBoard.climbExtend.whileTrue(superstructure.climbExtendCommand())
-    //    ControlBoard.climbRetract.whileTrue(superstructure.climbRetractCommand())
-    //    ControlBoard.requestIdle.whileTrue(superstructure.requestIdleCommand())
-    ControlBoard.runGroundIntake.whileTrue(ExamplePathAuto(drivetrain))
+    ControlBoard.runGroundIntake.whileTrue(superstructure.groundIntakeCommand())
+    ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand())
+    ControlBoard.prepAmpScore.whileTrue(superstructure.prepAmpCommand())
+    ControlBoard.ampScore.whileTrue(superstructure.scoreAmpCommand())
+    ControlBoard.scoreSpeakerLow.whileTrue(superstructure.scoreSpeakerLowCommand())
+    ControlBoard.scoreSpeakerMid.whileTrue(superstructure.scoreSpeakerMidCommand())
+    ControlBoard.scoreSpeakerHigh.whileTrue(superstructure.scoreSpeakerHighCommand())
+    ControlBoard.climbExtend.whileTrue(superstructure.climbExtendCommand())
+    ControlBoard.climbRetract.whileTrue(superstructure.climbRetractCommand())
+    ControlBoard.requestIdle.whileTrue(superstructure.requestIdleCommand())
+    //ControlBoard.runGroundIntake.whileTrue(ExamplePathAuto(drivetrain))
 
     /*
     TUNING COMMANDS

diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt
@@ -2,15 +2,16 @@ package com.team4099.robot2023.auto.mode
 
 import com.team4099.robot2023.auto.PathStore
 import com.team4099.robot2023.commands.drivetrain.FollowPathPlannerPathCommand
+import com.team4099.robot2023.commands.drivetrain.ResetPoseCommand
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
+import org.team4099.lib.geometry.Pose2d
 
 class ExamplePathAuto(val drivetrain: Drivetrain) : SequentialCommandGroup() {
   init {
     addRequirements(drivetrain)
     addCommands(
-      //      ResetPoseCommand(drivetrain,
-      // Pose2d(PathStore.examplePath.previewStartingHolonomicPose)),
+      ResetPoseCommand(drivetrain, Pose2d(PathStore.examplePath.previewStartingHolonomicPose)),
       FollowPathPlannerPathCommand(drivetrain, PathStore.examplePath)
     )
   }

diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt
@@ -138,7 +138,7 @@ class FollowPathPlannerPathCommand(
       PathPlannerHolonomicDriveController(
         translationPID,
         rotationPID,
-        DrivetrainConstants.MAX_AUTO_VEL,
+        DrivetrainConstants.MAX_MODULE_VEL,
         Translation2d(
           DrivetrainConstants.DRIVETRAIN_LENGTH / 2,
           DrivetrainConstants.DRIVETRAIN_WIDTH / 2
@@ -151,21 +151,23 @@ class FollowPathPlannerPathCommand(
   override fun initialize() {
     trajStartTime = Clock.fpgaTime
 
-    drivetrain.odometryPose = Pose2d(path.previewStartingHolonomicPose)
-
     currentSpeeds = drivetrain.targetedChassisSpeeds
     poseRotation = drivetrain.odometryPose.rotation.inRotation2ds
     generatedTrajectory = path.getTrajectory(currentSpeeds, poseRotation)
+
+    Logger.recordOutput("ActiveCommands/FollowPathPlannerPathCommand", true)
+    println("Pose init ${drivetrain.odometryPose.x}")
   }
 
   override fun execute() {
     if (thetakP.hasChanged() || thetakI.hasChanged() || thetakD.hasChanged()) {
       rotationPID = PathPlannerRotationPID(thetakP.get(), thetakI.get(), thetakD.get())
+      rotationPID.pplibRotationConstants
       swerveDriveController =
         PathPlannerHolonomicDriveController(
           translationPID,
           rotationPID,
-          DrivetrainConstants.MAX_AUTO_VEL,
+          DrivetrainConstants.MAX_MODULE_VEL,
           Translation2d(
             DrivetrainConstants.DRIVETRAIN_LENGTH / 2,
             DrivetrainConstants.DRIVETRAIN_WIDTH / 2
@@ -181,7 +183,7 @@ class FollowPathPlannerPathCommand(
         PathPlannerHolonomicDriveController(
           translationPID,
           rotationPID,
-          DrivetrainConstants.MAX_AUTO_VEL,
+          DrivetrainConstants.MAX_MODULE_VEL,
           Translation2d(
             DrivetrainConstants.DRIVETRAIN_LENGTH / 2,
             DrivetrainConstants.DRIVETRAIN_WIDTH / 2
@@ -219,6 +221,8 @@ class FollowPathPlannerPathCommand(
     pathFollowRequest.chassisSpeeds = targetedChassisSpeeds.chassisSpeedsWPILIB
     drivetrain.currentRequest = pathFollowRequest
 
+    println("Drivetrain request ${drivetrain.odometryPose.x}")
+
     // Update trajectory time
     trajCurTime = Clock.fpgaTime - trajStartTime
   }
@@ -248,6 +252,8 @@ class FollowPathPlannerPathCommand(
         Request.DrivetrainRequest.OpenLoop(
           0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
         )
-    }
+    }4
+
+    Logger.recordOutput("ActiveCommands/FollowPathPlannerPathCommand", false)
   }
 }