Merge pull request #111 from strykeforce/rabsamu/auton-tuning

Rabsamu/auton tuning

Author: jhh

src/main/deploy/paths/BackupPath.toml

@@ -1,5 +1,5 @@
 
-max_velocity = 2.0 # m/s
+max_velocity = 1.0 # m/s
 max_acceleration = 2.0 # m/s/s
 start_velocity = 0.0 # m/s
 end_velocity = 0.0 # m/s

src/main/deploy/paths/TrenchPickupPath.toml

@@ -3,16 +3,16 @@ max_velocity = 1.55 # m/s
 max_acceleration = 2.0 # m/s/s
 start_velocity = 0.0 # m/s
 end_velocity = 0.0 # m/s
-is_reversed = true
+is_reversed = false
 
 [start_pose]
     x = 0.0
     y = 0.0
-    angle = 0.0
+    angle = 180.0
 [end_pose]
-    x = -5.6
+    x = -5.7
     y = 0.0
-    angle = 0.0
+    angle = 180.0
 
 [[internal_points]]
     x = -3.0

src/main/deploy/paths/TrenchReturnPath.toml

@@ -1,19 +1,19 @@
 
-max_velocity = 3.6 # m/s
-max_acceleration = 3.0 # m/s/s
+max_velocity = 2.0 # m/s
+max_acceleration = 2.0 # m/s/s
 start_velocity = 0.0 # m/s
 end_velocity = 0.0 # m/s
-is_reversed = false
+is_reversed = true
 
 [start_pose]
-    x = 0.0
+    x = -5.7
     y = 0.0
-    angle = 0.0
+    angle = 180.0
 [end_pose]
-    x = 3.75
+    x = -2.0
     y = 0.0
-    angle = 0.0
+    angle = 180.0
 
 [[internal_points]]
-    x = 2.0
+    x = -4.0
     y = 0.0

src/main/java/frc/robot/Constants.java

@@ -155,10 +155,10 @@ public static TalonFXConfiguration getDriveTalonConfig() {
   public static final class IntakeConstants {
 
     // Subsystem Specific
-    public static final double kSquidSpeed = -0.4;
-    public static final double kIntakeSpeed = -0.4;
-    public static final double kSquidShootSpeed = -0.75;
-    public static final double kIntakeShootSpeed = -0.75;
+    public static final double kSquidSpeed = -0.3;
+    public static final double kIntakeSpeed = -0.3;
+    public static final double kSquidShootSpeed = -0.65;
+    public static final double kIntakeShootSpeed = -0.65;
     public static final double kSquidEjectSpeed = 0.25;
     public static final double kIntakeEjectSpeed = 0.25;
     public static final int kStallVelocity = 100;

src/main/java/frc/robot/commands/auto/PathDriveCommand.java

@@ -86,9 +86,9 @@ public void initialize() {
   public void execute() {
     state = trajectory.sample(timer.get());
     odometryPose = driveSubsystem.getPoseMeters();
-    speeds = holonomicDriveController.calculate(odometryPose, state, new Rotation2d());
+    speeds = holonomicDriveController.calculate(odometryPose, state, Rotation2d.fromDegrees(180));
     driveSubsystem.move(
-        speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond, true);
+        speeds.vxMetersPerSecond, speeds.vyMetersPerSecond, speeds.omegaRadiansPerSecond, false);
   }
 
   @Override

src/main/java/frc/robot/commands/auto/TrenchAutoCommandGroup.java

@@ -17,7 +17,7 @@ public TrenchAutoCommandGroup(
       String ballFetchPath,
       String driveShootPath) {
     addCommands(
-        new OffsetGyroCommand(gyroOffset), // FIXME: Move to end
+        new OffsetGyroCommand(gyroOffset),
         new AutoArmCommandGroup(),
         new ParallelDeadlineGroup(new WaitCommand(shootWait), new ArmedShootSequenceCommand()),
         new ParallelDeadlineGroup(

src/main/java/frc/robot/commands/sequences/AutoIntakeCmdGroup.java

@@ -1,16 +1,18 @@
 package frc.robot.commands.sequences;
 
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import frc.robot.Constants;
 import frc.robot.Constants.MagazineConstants;
 import frc.robot.commands.LogCommand;
-import frc.robot.commands.intake.IntakeAutoStopCommand;
+import frc.robot.commands.intake.IntakeRunCommand;
 import frc.robot.commands.magazine.LimitMagazineCommand;
 
 public class AutoIntakeCmdGroup extends ParallelCommandGroup {
   public AutoIntakeCmdGroup() {
     addCommands(
         new LogCommand("Begin Driver Controller - Auto Intake"),
         new LimitMagazineCommand(MagazineConstants.kOpenLoopLoad),
-        new IntakeAutoStopCommand());
+        new IntakeRunCommand(
+            Constants.IntakeConstants.kIntakeSpeed, Constants.IntakeConstants.kSquidSpeed));
   }
 }

src/main/java/frc/robot/controls/DriverControls.java

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import frc.robot.commands.auto.PathDriveCommand;
 import frc.robot.commands.drive.XLockCommand;
 import frc.robot.commands.drive.ZeroGyroCommand;
 import frc.robot.commands.hood.HoodOpenLoopCommand;
@@ -34,7 +35,8 @@ public class DriverControls {
 
     // Software Testing
 
-    new JoystickButton(joystick, Button.HAMBURGER.id).whenPressed(new XLockCommand());
+    new JoystickButton(joystick, Button.HAMBURGER.id)
+        .whenPressed(new PathDriveCommand("BackupPath"));
   }
   /** Left stick X (up-down) axis. */
   public double getForward() {

vendordeps/thirdcoast.json

@@ -1,7 +1,7 @@
 {
     "fileName": "thirdcoast.json",
     "name": "StrykeForce-ThirdCoast",
-    "version": "21.3.1-SNAPSHOT",
+    "version": "21.3.1",
     "uuid": "13c4f4b5-a1c0-4670-8f8d-b7b03628c0d3",
     "mavenUrls": [
         "http://maven.strykeforce.org/repo"
@@ -11,9 +11,9 @@
         {
             "groupId": "org.strykeforce",
             "artifactId": "thirdcoast",
-            "version": "21.3.1-SNAPSHOT"
+            "version": "21.3.1"
         }
     ],
     "jniDependencies": [],
     "cppDependencies": []
-}
+}