autos better + drive for time + rotate until target

src/main/java/frc4388/robot/Constants.java

@@ -46,18 +46,18 @@ public final class Constants {
     public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; // TODO: redundant constant?
 
     // IDs
-    public static final int LEFT_FRONT_STEER_CAN_ID = 2;           // *
-    public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;           // *
-    public static final int RIGHT_FRONT_STEER_CAN_ID = 4;          // *
-    public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5;          // *
-    public static final int LEFT_BACK_STEER_CAN_ID = 6;            // *
-    public static final int LEFT_BACK_WHEEL_CAN_ID = 7;            // *
-    public static final int RIGHT_BACK_STEER_CAN_ID = 8;           // *
-    public static final int RIGHT_BACK_WHEEL_CAN_ID = 9;           // *
-    public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10;  // *
-    public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; // *
-    public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;   // *
-    public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;  // *
+    public static final int LEFT_FRONT_STEER_CAN_ID = 2;
+    public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
+    public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
+    public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5;
+    public static final int LEFT_BACK_STEER_CAN_ID = 6;
+    public static final int LEFT_BACK_WHEEL_CAN_ID = 7;
+    public static final int RIGHT_BACK_STEER_CAN_ID = 8;
+    public static final int RIGHT_BACK_WHEEL_CAN_ID = 9;
+    public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10;
+    public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
+    public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
+    public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
     public static final int GYRO_ID = 14;
 
     // offsets are in degrees
@@ -85,8 +85,8 @@ public final class Constants {
     // swerve auto constants
     public static final PIDController X_CONTROLLER = new PIDController(0.5, 0.0, 0.0);
     public static final PIDController Y_CONTROLLER = new PIDController(2.0, 0.0, 0.0);
-    public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(15.0, 0.1, 0.3,
-        new TrapezoidProfile.Constraints(Math.PI, Math.PI));
+    public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(2.0, 0, 0.01,//0.1, 0.3,
+        new TrapezoidProfile.Constraints(Math.PI, Math.PI/2));
     public static final boolean PATH_RECORD_VELOCITY = true;
     public static final double PATH_MAX_VELOCITY = 5.0;
     public static final double PATH_MAX_ACCELERATION = 5.0;

src/main/java/frc4388/robot/Robot.java

@@ -213,8 +213,7 @@ public class Robot extends TimedRobot {
    * This function is called periodically during autonomous.
    */
   @Override
-  public void autonomousPeriodic() {
-  }
+  public void autonomousPeriodic() {}
 
   @Override
   public void teleopInit() {

src/main/java/frc4388/robot/RobotContainer.java

@@ -46,6 +46,8 @@ import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.Trajectory.State;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.Filesystem;
@@ -59,6 +61,7 @@ import edu.wpi.first.wpilibj2.command.NotifierCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
 import frc4388.robot.subsystems.Claws;
@@ -71,7 +74,8 @@ import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch;
 import frc4388.robot.commands.ClimberCommands.RunClaw;
 import frc4388.robot.commands.ClimberCommands.RunClimberPath;
-// import frc4388.robot.commands.ButtonBoxCommands.TurretManual;
+import frc4388.robot.commands.DriveCommands.DriveWithInputForTime;
+import frc4388.robot.commands.DriveCommands.RotateUntilTarget;
 import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
 import frc4388.robot.commands.ShooterCommands.AimToCenter;
 import frc4388.robot.commands.ShooterCommands.Seek;
@@ -97,6 +101,7 @@ import frc4388.utility.PathPlannerUtil;
 import frc4388.utility.Vector2D;
 import frc4388.utility.PathPlannerUtil.Path.Waypoint;
 import frc4388.utility.controller.ButtonBox;
+import frc4388.utility.controller.DeadbandedRawXboxController;
 import frc4388.utility.controller.DeadbandedXboxController;
 
 /**
@@ -128,8 +133,8 @@ public class RobotContainer {
   public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
 
   // Controllers
-  private final static XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
-  private final static XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
+  private final static XboxController m_driverXbox = new DeadbandedRawXboxController(OIConstants.XBOX_DRIVER_ID);
+  private final static XboxController m_operatorXbox = new DeadbandedRawXboxController(OIConstants.XBOX_OPERATOR_ID);
   private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID);
 
   // Autonomous
@@ -371,8 +376,11 @@ public class RobotContainer {
       if (inputs[i] instanceof String) {
 
         PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
-        PathPlannerState initState = (PathPlannerState) traj.sample(0);
-        commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation))));
+        PathPlannerState initState = traj.getInitialState();
+
+        Pose2d initPose = new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation);
+
+        commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initPose), m_robotSwerveDrive));
         commands.add(new PPSwerveControllerCommand(
                           traj,
                           m_robotSwerveDrive::getOdometry,
@@ -383,12 +391,13 @@ public class RobotContainer {
                           m_robotSwerveDrive::setModuleStates,
                           m_robotSwerveDrive));
       }
+
       if (inputs[i] instanceof Command) {
         commands.add((Command) inputs[i]);
       }
     }
     
-    commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules()));
+    commands.add(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, true), m_robotSwerveDrive));
     Command[] ret = new Command[commands.size()];
     ret = commands.toArray(ret);
     return ret;
@@ -421,6 +430,13 @@ public class RobotContainer {
     //   }).withName("No Autonomous Path");
     // }
 
+    PIDController xController = SwerveDriveConstants.X_CONTROLLER;
+    PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
+    ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
+    // thetaController.enableContinuousInput(-Math.PI, Math.PI);
+
+    loadPath("Move Forward");
+
     // ! this will run each of the specified PathPlanner paths in sequence.
     // * return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3"));
 
@@ -429,8 +445,16 @@ public class RobotContainer {
     // *                                           null,
     // *                                           new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")),
     // *                                           new RunCommand(() -> m_robotIntake.runAtOutput(0.5))));
+    
+    // return new SequentialCommandGroup(buildAuto(1.0, 1.0, new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset(), m_robotSwerveDrive),
+    //                                                       // new InstantCommand(() -> this.resetOdometry(new Pose2d())),
+    //                                                       new InstantCommand(() -> m_robotSwerveDrive.setModuleRotationsToAngle(0.0), m_robotSwerveDrive), 
+    //                                                       "Diamond"));
 
-    return new SequentialCommandGroup(buildAuto(1.0, 1.0, new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()), "Move Forward")); // test command
+    return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
+                                       new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
+                                       new RotateUntilTarget(m_robotSwerveDrive, m_robotVisionOdometry, 0.5),
+                                       new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
   }
 
   public static XboxController getDriverController() {
@@ -563,7 +587,7 @@ public class RobotContainer {
   }
 
   private void saveRecording() {
-    // IMPORTANT: Had to chown the pathplanner folder in order to save autos.
+    // ! IMPORTANT: Had to chown the pathplanner folder in order to save autos.
     File outputFile = PATHPLANNER_DIRECTORY
         .resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile();
     LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath());
@@ -584,8 +608,7 @@ public class RobotContainer {
   public void recordPeriodic() {
     Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation();
     Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d();
-    // FIXME: Chassis speeds are created from joystick inputs and do not reflect
-    // actual robot velocity.
+    // * FIXME: Chassis speeds are created from joystick inputs and do not reflect actual robot velocity
     Translation2d velocity = new Translation2d(m_robotSwerveDrive.getChassisSpeeds()[0],
         m_robotSwerveDrive.getChassisSpeeds()[1]);
     Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false,

src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java

@@ -0,0 +1,59 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.DriveCommands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class DriveWithInputForTime extends CommandBase {
+
+  // subsystems
+  SwerveDrive swerve;
+
+  double[] inputs;
+
+  // timing
+  long start;
+  long elapsed;
+  double duration;
+
+  /**
+   * DriveWithInput for a specified amount of time.
+   * @param inputs Inputs used in DriveWithInput (xspeed, yspeed, xrot, yrot).
+   * @param time Time to DriveWithInput for, in seconds.
+   */
+  public DriveWithInputForTime(SwerveDrive swerve, double[] inputs, double duration) {
+    // Use addRequirements() here to declare subsystem dependencies.
+
+    this.swerve = swerve;
+    this.inputs = inputs;
+    this.duration = duration;
+
+    addRequirements(this.swerve);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    start = System.currentTimeMillis();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    elapsed = System.currentTimeMillis() - start;
+    this.swerve.driveWithInput(inputs[0], inputs[1], inputs[2], inputs[3], true);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return (((long) duration) >= (start - elapsed));
+  }
+}

src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java

@@ -0,0 +1,52 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.DriveCommands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.VisionOdometry;
+
+public class RotateUntilTarget extends CommandBase {
+
+  // subsystems
+  SwerveDrive swerve;
+  VisionOdometry visionOdometry;
+
+  double rotateSpeed;
+
+  /** Creates a new RotateUntilTarget. */
+  public RotateUntilTarget(SwerveDrive swerve, VisionOdometry visionOdometry, double rotateSpeed) {
+    // Use addRequirements() here to declare subsystem dependencies.
+
+    this.swerve = swerve;
+    this.visionOdometry = visionOdometry;
+
+    this.rotateSpeed = rotateSpeed;
+
+    addRequirements(this.swerve, this.visionOdometry);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    this.visionOdometry.setLEDs(true);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    this.swerve.driveWithInput(0.0, 0.0, rotateSpeed, true);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return this.visionOdometry.m_camera.getLatestResult().hasTargets();
+  }
+}

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -158,6 +158,13 @@ public class SwerveDrive extends SubsystemBase {
     // modules[0].setDesiredState(desiredStates[0], false);
   }
 
+  public void setModuleRotationsToAngle(double angle) {
+    for (int i = 0; i < modules.length; i++) {
+      SwerveModule module = modules[i];
+      module.rotateToAngle(angle);
+    }
+  }
+
   @Override
   public void periodic() {
 
@@ -228,10 +235,16 @@ public class SwerveDrive extends SubsystemBase {
    * Updates the field relative position of the robot.
    */
   public void updateOdometry() {
-    m_poseEstimator.update( getRegGyro(),
-                            modules[0].getState(), 
-                            modules[1].getState(), 
-                            modules[2].getState(), 
+    Rotation2d actualDWI = new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI/2));
+    Rotation2d actual =  new Rotation2d(-1 * m_gyro.getRotation2d().getRadians());
+
+    SmartDashboard.putNumber("AUTO ACTUAL GYRO", actual.getDegrees());
+    SmartDashboard.putNumber("AUTO DWI GYRO", actual.getDegrees());
+
+    m_poseEstimator.update( actualDWI,//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()),
+                            modules[0].getState(),
+                            modules[1].getState(),
+                            modules[2].getState(),
                             modules[3].getState());
   }
   

src/main/java/frc4388/robot/subsystems/SwerveModule.java

@@ -182,6 +182,10 @@ public class SwerveModule extends SubsystemBase {
     angleMotor.set(0);
   }
 
+  public void rotateToAngle(double angle) {
+    this.angleMotor.set(TalonFXControlMode.Position, angle);
+  }
+
   @Override
   public void periodic() {