Atonomous tuning

PathWeaver/Groups/Unnamed

@@ -0,0 +1,2 @@
+Unnamed.path
+Unnamed_0.path

PathWeaver/Paths/Unnamed.path

@@ -1,4 +1,12 @@
 X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
-3.1168919451384496,-2.3970364635190977,-3.170680979338714,0.0,true,false,
-1.6811118790228055,-1.320201413932365,0.5384175247933667,1.9143734214875257,true,false,
-9.506113239353066,-0.7458893874861073,2.6442282884296446,-0.023929667768594287,true,false,
+0.7704815505307601,-5.700648459765242,2.5937258190847863,0.0,true,false,
+6.576620081326062,-5.474589787459688,3.227750867421687,-0.3744769825606934,false,false,
+10.491004459669618,-6.355028826965532,2.0345280507499925,0.0951825988654953,true,false,
+11.478523922899145,-5.807728883488926,0.16656954801462298,0.28554779659648943,true,false,
+11.728378244921075,-4.784515945684835,-0.16656954801461943,1.5705128812806959,true,false,
+10.288741437080438,-3.975463855328114,-2.0107324010336196,-0.2974456214546777,true,false,
+8.254213386330443,-4.89159636940852,-1.070804237236839,-0.30934344631286415,true,false,
+5.39873542036554,-5.391305013452377,-1.6775933050043799,-0.582993418051168,true,false,
+2.888294375288063,-6.31933535239097,-2.8078866665321542,0.27364997173830297,true,false,
+1.317781494007366,-4.879698544550332,0.5948912429093549,1.2849650846842064,true,false,
+2.8644987255716883,-4.213420352491854,1.0470085875204656,0.9637238135131536,true,false,

PathWeaver/Paths/Unnamed_0.path

@@ -1,6 +1,3 @@
 X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
-0.7823793753889469,-2.6310096463529717,3.048,0.0,true,false,
-3.8758138385175918,-2.96414874238221,0.5591977683347937,-1.7846737287280647,true,false,
-3.161944347026366,-4.27290947678279,-1.8679585027353736,-0.6543803672002904,true,false,
-0.009020759606786008,-4.130135578484547,-0.8328477400730968,1.3206585592587672,true,false,
-0.782,-2.631,3.048,0.0,true,false,
+2.864,-4.213,1.047,0.964,true,true,
+0.77,-5.701,2.3200758473464838,0.0,true,true,

PathWeaver/pathweaver.json

@@ -1,7 +1,7 @@
 {
   "lengthUnit": "Meter",
   "exportUnit": "Always Meters",
-  "maxVelocity": 1.0,
+  "maxVelocity": 2.0,
   "maxAcceleration": 1.0,
   "wheelBase": 0.5969,
   "gameName": "Infinite Recharge",

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

@@ -77,7 +77,7 @@ public final static class Drive {
         public static final int RIGHT_SLAVE_ID = 1;
 
         /******** PID Constants ********/
-        public static final PIDParameters WHEEL_PID_CONSTANTS = new PIDParameters(0.4, 0.01, 0);
+        public static final PIDParameters WHEEL_PID_CONSTANTS = new PIDParameters(0.3, 0.0001, .6);
         public static final PIDController ROTATION_PID_CONTROLLER = new PIDController(0.02, 0, 0.0005);
 
         /******** Physical Constants ********/

src/main/java/frc/robot/RobotContainer.java

@@ -7,12 +7,15 @@
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.GenericHID.Hand;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.commands.AimAndShoot;
 import frc.robot.commands.Climb;
 import frc.robot.commands.DriveTeleop;
 import frc.robot.commands.DriveTest;
 import frc.robot.commands.Intake;
 import frc.robot.commands.autos.FollowTrajectory;
+import frc.robot.commands.autos.TestAuto;
 import frc.robot.subsystems.ArmSubsystem;
 import frc.robot.subsystems.ClimberSubsystem;
 import frc.robot.subsystems.DriveSubsystem;
@@ -40,10 +43,16 @@ public class RobotContainer {
   private final SerializerSubsystem serializer = new SerializerSubsystem();
   private final ShooterSubsystem shooter = new ShooterSubsystem();
 
+  private SendableChooser<Command> autoChooser = new SendableChooser<Command>();
+  private final TestAuto testTrajectory = new TestAuto(drive);
+
   /** 
    * The container for the robot. Contains subsystems, OI devices, and commands. 
    */
   public RobotContainer() {
+    autoChooser.setDefaultOption("Test auto", testTrajectory);
+    SmartDashboard.putData(autoChooser);
+
     configureDefaultCommands();
     configureButtonBindings();
   }
@@ -84,7 +93,6 @@ private void configureButtonBindings() {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-    // An ExampleCommand will run in autonomous
-    return null;//m_autoCommand;
+    return autoChooser.getSelected();
   }
 }

src/main/java/frc/robot/commands/autos/FollowTrajectory.java

@@ -24,22 +24,26 @@
  */
 public class FollowTrajectory extends CommandBase {
 
-    DriveSubsystem drive;
+    private DriveSubsystem drive;
 
-    RamseteController ramseteController;
+    private boolean resetOdometry;
+
+    private RamseteController ramseteController;
     private Timer timer = new Timer();
     private Trajectory trajectory;
 
     private final Field2d f2d = new Field2d();
 
-    public FollowTrajectory (DriveSubsystem drive, String trajectoryJSON) {
+    public FollowTrajectory (DriveSubsystem drive, String trajectoryJSON, boolean resetOdometry) {
         this.drive = drive;
         addRequirements(drive);
 
+        this.resetOdometry = resetOdometry;
+
         ramseteController = new RamseteController();
 
         // Set the range where the ramsete controller considers itself at its target location
-        ramseteController.setTolerance(new Pose2d(new Translation2d(.09, .09), Rotation2d.fromDegrees(10)));
+        ramseteController.setTolerance(new Pose2d(new Translation2d(0.4, 0.4), Rotation2d.fromDegrees(30)));
 
         trajectory = openTrajectoryFromJSON(trajectoryJSON); //Load the pathweaver trajectory
 
@@ -49,7 +53,9 @@ public FollowTrajectory (DriveSubsystem drive, String trajectoryJSON) {
     // Called when the command is initially scheduled.
     @Override
     public void initialize() {
-        drive.resetOdometry(trajectory.getInitialPose(), trajectory.getInitialPose().getRotation()); // Re-zero the robot's odometry
+        if (resetOdometry) {
+            drive.resetOdometry(trajectory.getInitialPose(), trajectory.getInitialPose().getRotation()); // Re-zero the robot's odometry
+        }
         timer.reset();
         timer.start();
     }

src/main/java/frc/robot/commands/autos/TestAuto.java

@@ -0,0 +1,13 @@
+package frc.robot.commands.autos;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.subsystems.DriveSubsystem;
+
+public class TestAuto extends SequentialCommandGroup {
+    public TestAuto (DriveSubsystem drive) {
+        addCommands(
+            new FollowTrajectory(drive, "Unnamed.wpilib.json", true),
+            new FollowTrajectory(drive, "Unnamed_0.wpilib.json", false)
+        );
+    }
+}

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -2,6 +2,7 @@
 
 import com.analog.adis16470.frc.ADIS16470_IMU;
 import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
 import edu.wpi.first.wpilibj.geometry.Pose2d;
@@ -103,10 +104,12 @@ public void stop () {
     private void configureMotors (WPI_TalonFX master, WPI_TalonFX slave, boolean inverted) {
         master.configFactoryDefault();
         master.setInverted(inverted);
+        master.setNeutralMode(NeutralMode.Brake);
         PIDParameters.configureMotorPID(master, Constants.Drive.WHEEL_PID_CONSTANTS);
 
         slave.configFactoryDefault();
         slave.setInverted(inverted);
+        slave.setNeutralMode(NeutralMode.Brake);
         slave.follow(master);
     }