3/4/24

frc1418 · Mar 4, 2024 · de6f84e · de6f84e · MyaTaheri · Mar 4, 2024
1 parent 678ebcc
commit de6f84e
Show file tree

Hide file tree

Showing 10 changed files with 269 additions and 19 deletions.
diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
@@ -0,0 +1,12 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "maxModuleSpeed": 4.5
+}
diff --git a/src/main/deploy/pathplanner/paths/Left2NoteBlue.path b/src/main/deploy/pathplanner/paths/Left2NoteBlue.path
@@ -0,0 +1,70 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.9323613212443815,
+        "y": 6.656317475275852
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.9323613212443809,
+        "y": 6.656317475275852
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.6751120813030673,
+        "y": 6.984842182244751
+      },
+      "prevControl": {
+        "x": 1.6751120813030673,
+        "y": 6.984842182244751
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [
+    {
+      "name": "print",
+      "waypointRelativePos": 0.75,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "printLeft2Note"
+              }
+            }
+          ]
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 0.5,
+    "maxAcceleration": 0.5,
+    "maxAngularVelocity": 90.0,
+    "maxAngularAcceleration": 90.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": -120.0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": false
+}
diff --git a/src/main/deploy/pathplanner/paths/Middle2Note.path b/src/main/deploy/pathplanner/paths/Middle2Note.path
@@ -0,0 +1,70 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.3233687216166703,
+        "y": 5.562302316231041
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.323368721616671,
+        "y": 5.562302316231041
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.8958670517985157,
+        "y": 5.562302316231041
+      },
+      "prevControl": {
+        "x": 1.8958670517985157,
+        "y": 5.562302316231041
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [
+    {
+      "name": "print",
+      "waypointRelativePos": 0.9,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "print"
+              }
+            }
+          ]
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
diff --git a/src/main/deploy/pathplanner/paths/Right2NoteRed.path b/src/main/deploy/pathplanner/paths/Right2NoteRed.path
@@ -0,0 +1,70 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 15.781810114583042,
+        "y": 6.7769344718348545
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 14.197917918710854,
+        "y": 6.876687903298562
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 13.91634883530184,
+        "y": 6.975960595336369
+      },
+      "prevControl": {
+        "x": 14.959553395669293,
+        "y": 6.829575439284806
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [
+    {
+      "name": "print",
+      "waypointRelativePos": 0.55,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": [
+            {
+              "type": "named",
+              "data": {
+                "name": "printRight2NoteRed"
+              }
+            }
+          ]
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": -60.0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": false
+}
diff --git a/src/main/deploy/pathplanner/paths/SimpleBack.path b/src/main/deploy/pathplanner/paths/SimpleBack.path
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.7609567117282383,
-        "y": 6.0
+        "x": 1.0,
+        "y": 1.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.7609567117282374,
-        "y": 6.0
+        "x": 1.9999999999999991,
+        "y": 1.0
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 4.0,
-        "y": 6.0
+        "x": 2.0,
+        "y": 1.0
       },
       "prevControl": {
-        "x": 3.0,
-        "y": 6.0
+        "x": 1.0,
+        "y": 1.0
       },
       "nextControl": null,
       "isLocked": false,
@@ -30,12 +30,23 @@
   ],
   "rotationTargets": [],
   "constraintZones": [],
-  "eventMarkers": [],
+  "eventMarkers": [
+    {
+      "name": "New Event Marker",
+      "waypointRelativePos": 0,
+      "command": {
+        "type": "parallel",
+        "data": {
+          "commands": []
+        }
+      }
+    }
+  ],
   "globalConstraints": {
     "maxVelocity": 1.0,
     "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0
+    "maxAngularVelocity": 90.0,
+    "maxAngularAcceleration": 90.0
   },
   "goalEndState": {
     "velocity": 0,

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -77,7 +77,7 @@ public final static class DriverConstants {
   }
 
   public final static class WheelConstants {
-    public final static double ROTATIONS_TO_METERS = 0.33/8.33;
+    public final static double ROTATIONS_TO_METERS = 0.33/8.33 * 1.17;
   }
 
       public enum LimelightDirections {

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -63,9 +63,9 @@ public void disabledPeriodic() {}
   @Override
   public void autonomousInit() {
     System.out.println("STARTING AUTO");
-    // m_robotContainer.getOdometry().zeroHeading();
-    // m_robotContainer.getOdometry().reset(new Pose2d(0, 0, Rotation2d.fromDegrees(0)));
-    // m_robotContainer.getSwerveDriveSubsystem().resetLockRot();
+    m_robotContainer.getOdometry().zeroHeading();
+    m_robotContainer.getOdometry().reset(new Pose2d(0, 0, Rotation2d.fromDegrees(0)));
+    m_robotContainer.getSwerveDriveSubsystem().resetLockRot();
     System.out.println("GET AUTO");
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
     System.out.println("AUTO GOT!");
@@ -89,6 +89,7 @@ public void teleopInit() {
     // this line or comment it out.
     m_robotContainer.getOdometry().zeroHeading();
     m_robotContainer.getSwerveDriveSubsystem().resetLockRot();
+    m_robotContainer.getOdometry().reset(new Pose2d(0, 0, Rotation2d.fromDegrees(0)));
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -19,6 +19,7 @@
 import frc.robot.subsystems.MaxWheelModule;
 import frc.robot.subsystems.PivotSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
+
 import com.kauailabs.navx.frc.AHRS;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkBase.IdleMode;
@@ -34,6 +35,7 @@
 import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
@@ -120,7 +122,7 @@ public class RobotContainer {
     private final AlignByAprilTag alignLeftOfSpeaker = new AlignByAprilTag(swerveDrive, limelight, odometry,  -1.08, -0.96, 1, 0.04, 0.1, -44, -4);
 
     private final ChargeCommand chargeCommand;
-
+    
     SlewRateLimiter limitX = new SlewRateLimiter(6);
     SlewRateLimiter limitY = new SlewRateLimiter(6);
     //Limits shooter motor speed
@@ -134,7 +136,10 @@ public RobotContainer(RobotBase robot) {
 
     CameraServer.startAutomaticCapture();
 
-    NamedCommands.registerCommand("toggleFieldCentric", swerveDrive.toggleFieldCentric());
+    NamedCommands.registerCommand("print", new PrintCommand("print!"));
+    NamedCommands.registerCommand("printRight2NoteRed", new PrintCommand("righ2notered path!"));
+    NamedCommands.registerCommand("printLeft2Note", new PrintCommand("left2note path!"));
+
 
     // Configure the trigger bindings
     configureBindings();

diff --git a/src/main/java/frc/robot/commands/autonomous/ChargeCommand.java b/src/main/java/frc/robot/commands/autonomous/ChargeCommand.java
@@ -20,14 +20,15 @@
 /** An example command that uses an example subsystem. */
 public class ChargeCommand extends SequentialCommandGroup {
 
-    private String TRAJECTORY_NAME = "SimpleBack";
+    private String TRAJECTORY_NAME = "Middle2Note";
     SwerveDriveSubsystem swerveDrive;
     FeedSubsystem feedSubsystem;
 
     public ChargeCommand(SwerveDriveSubsystem swerveDrive, FeedSubsystem feedSubsystem) {
       this.swerveDrive = swerveDrive;
       this.feedSubsystem = feedSubsystem;
 
+
       addRequirements(swerveDrive, feedSubsystem);
 
       addCommands(