Fix many things

lasarobotics · Feb 27, 2024 · 550b048 · 550b048
1 parent b986a24
commit 550b048
Show file tree

Hide file tree

Showing 9 changed files with 34 additions and 15 deletions.
diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
@@ -4,8 +4,9 @@
   "holonomicMode": true,
   "pathFolders": [],
   "autoFolders": [],
-  "defaultMaxVel": 4.3,
+  "defaultMaxVel": 2.3,
   "defaultMaxAccel": 4.3,
   "defaultMaxAngVel": 1080.0,
-  "defaultMaxAngAccel": 360.0
+  "defaultMaxAngAccel": 360.0,
+  "maxModuleSpeed": 4.5
 }
diff --git a/src/main/deploy/pathplanner/paths/Leave.path b/src/main/deploy/pathplanner/paths/Leave.path
@@ -38,7 +38,7 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 0.3,
+    "maxVelocity": 2.3,
     "maxAcceleration": 4.3,
     "maxAngularVelocity": 1080.0,
     "maxAngularAcceleration": 360.0

diff --git a/src/main/deploy/pathplanner/paths/pick up first note ring.path b/src/main/deploy/pathplanner/paths/pick up first note ring.path
@@ -32,7 +32,7 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 0.3,
+    "maxVelocity": 2.3,
     "maxAcceleration": 4.3,
     "maxAngularVelocity": 1080.0,
     "maxAngularAcceleration": 360.0

diff --git a/src/main/deploy/pathplanner/paths/pickup second note ring.path b/src/main/deploy/pathplanner/paths/pickup second note ring.path
@@ -54,7 +54,7 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 0.3,
+    "maxVelocity": 2.3,
     "maxAcceleration": 4.3,
     "maxAngularVelocity": 1080.0,
     "maxAngularAcceleration": 360.0

diff --git a/src/main/deploy/pathplanner/paths/pickup third note ring.path b/src/main/deploy/pathplanner/paths/pickup third note ring.path
@@ -32,7 +32,7 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 0.3,
+    "maxVelocity": 2.3,
     "maxAcceleration": 4.3,
     "maxAngularVelocity": 1080.0,
     "maxAngularAcceleration": 360.0

diff --git a/src/main/deploy/pathplanner/paths/shoot preload ring.path b/src/main/deploy/pathplanner/paths/shoot preload ring.path
@@ -38,7 +38,7 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 0.3,
+    "maxVelocity": 2.3,
     "maxAcceleration": 4.3,
     "maxAngularVelocity": 1080.0,
     "maxAngularAcceleration": 360.0

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -68,7 +68,7 @@ public static class HID {
 
   public static class Drive {
     public static final PIDConstants DRIVE_ROTATE_PID = new PIDConstants(7.0, 0.0, 0.1, 0.0, 0.0);
-    public static final double DRIVE_SLIP_RATIO = 0.11;
+    public static final double DRIVE_SLIP_RATIO = 0.12;
     public static final double DRIVE_TURN_SCALAR = 60.0;
     public static final double DRIVE_LOOKAHEAD = 6;
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -7,10 +7,13 @@
 import com.revrobotics.REVPhysicsSim;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.subsystems.WaggleSubsystem;
+import frc.robot.subsystems.drive.AutoTrajectory;
 import frc.robot.subsystems.drive.DriveSubsystem;
 
 public class RobotContainer {
@@ -29,6 +32,8 @@ public class RobotContainer {
 
   private static final CommandXboxController PRIMARY_CONTROLLER = new CommandXboxController(Constants.HID.PRIMARY_CONTROLLER_PORT);
 
+  private static SendableChooser<Command> m_automodeChooser = new SendableChooser<>();
+
   public RobotContainer() {
     // Set drive command
     DRIVE_SUBSYSTEM.setDefaultCommand(
@@ -42,6 +47,9 @@ public RobotContainer() {
     // Setup AutoBuilder
     DRIVE_SUBSYSTEM.configureAutoBuilder();
 
+    autoModeChooser();
+    SmartDashboard.putData(Constants.SmartDashboard.SMARTDASHBOARD_AUTO_MODE, m_automodeChooser);
+
     // Bind buttons and triggers
     configureBindings();
   }
@@ -74,11 +82,21 @@ public void simulationPeriodic() {
     REVPhysicsSim.getInstance().run();
   }
 
+    /**
+   * Add auto modes to chooser
+   */
+  private void autoModeChooser() {
+    m_automodeChooser.setDefaultOption("Do nothing", Commands.none());
+    m_automodeChooser.addOption("Leave", new AutoTrajectory(DRIVE_SUBSYSTEM, "Leave").getCommand());
+    m_automodeChooser.addOption("Preload + 3 Ring", new AutoTrajectory(DRIVE_SUBSYSTEM, "Preload + 3 Ring").getCommand());
+    m_automodeChooser.addOption("Preload + 1", new AutoTrajectory(DRIVE_SUBSYSTEM, "Preload + 1").getCommand());
+  }
+
   /**
    * Get currently selected autonomous command
    * @return Autonomous command
    */
   public Command getAutonomousCommand() {
-    return Commands.print("No autonomous command configured");
+    return m_automodeChooser.getSelected();
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
@@ -94,8 +94,8 @@ public Hardware(NavX2 navx,
   public static final Measure<Distance> DRIVE_WHEELBASE = Units.Meters.of(0.62);
   public static final Measure<Distance> DRIVE_TRACK_WIDTH = Units.Meters.of(0.62);
   public static final Measure<Time> AUTO_LOCK_TIME = Units.Seconds.of(3.0);
-  public static final Measure<Time> MAX_SLIPPING_TIME = Units.Seconds.of(0.9);
-  public static final Measure<Current> DRIVE_CURRENT_LIMIT = Units.Amps.of(45.0);
+  public static final Measure<Time> MAX_SLIPPING_TIME = Units.Seconds.of(1.0);
+  public static final Measure<Current> DRIVE_CURRENT_LIMIT = Units.Amps.of(30.0);
   public static final Measure<Velocity<Angle>> NAVX2_YAW_DRIFT_RATE = Units.DegreesPerSecond.of(0.5 / 60);
   public static final Measure<Velocity<Angle>> DRIVE_ROTATE_VELOCITY = Units.RadiansPerSecond.of(12 * Math.PI);
   public static final Measure<Velocity<Velocity<Angle>>> DRIVE_ROTATE_ACCELERATION = Units.RadiansPerSecond.of(4 * Math.PI).per(Units.Second);
@@ -285,7 +285,7 @@ public static Hardware initializeHardware() {
       MAXSwerveModule.initializeHardware(
         Constants.DriveHardware.LEFT_FRONT_DRIVE_MOTOR_ID,
         Constants.DriveHardware.LEFT_FRONT_ROTATE_MOTOR_ID,
-        MotorKind.NEO_VORTEX
+        MotorKind.NEO
       ),
       MAXSwerveModule.ModuleLocation.LeftFront,
       Constants.Drive.GEAR_RATIO,
@@ -301,7 +301,7 @@ public static Hardware initializeHardware() {
       MAXSwerveModule.initializeHardware(
         Constants.DriveHardware.RIGHT_FRONT_DRIVE_MOTOR_ID,
         Constants.DriveHardware.RIGHT_FRONT_ROTATE_MOTOR_ID,
-        MotorKind.NEO_VORTEX
+        MotorKind.NEO
       ),
       MAXSwerveModule.ModuleLocation.RightFront,
       Constants.Drive.GEAR_RATIO,
@@ -317,7 +317,7 @@ public static Hardware initializeHardware() {
       MAXSwerveModule.initializeHardware(
         Constants.DriveHardware.LEFT_REAR_DRIVE_MOTOR_ID,
         Constants.DriveHardware.LEFT_REAR_ROTATE_MOTOR_ID,
-        MotorKind.NEO_VORTEX
+        MotorKind.NEO
       ),
       MAXSwerveModule.ModuleLocation.LeftRear,
       Constants.Drive.GEAR_RATIO,
@@ -333,7 +333,7 @@ public static Hardware initializeHardware() {
       MAXSwerveModule.initializeHardware(
         Constants.DriveHardware.RIGHT_REAR_DRIVE_MOTOR_ID,
         Constants.DriveHardware.RIGHT_REAR_ROTATE_MOTOR_ID,
-        MotorKind.NEO_VORTEX
+        MotorKind.NEO
       ),
       MAXSwerveModule.ModuleLocation.RightRear,
       Constants.Drive.GEAR_RATIO,