Merge pull request #31 from Team4388/autonomous

Autonomous --> Master

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

@@ -47,18 +47,17 @@ public final class Constants {
     public static final class PIDConstants {
       public static final int SWERVE_SLOT_IDX = 0;
       public static final int SWERVE_PID_LOOP_IDX = 1;
-      public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0);
+      public static final Gains SWERVE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0);
     }
 
     public static final class AutoConstants {
-      public static final PIDController X_CONTROLLER = new PIDController(0.0, 0.0, 0.0);
+      public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0);
-      public static final PIDController Y_CONTROLLER = new PIDController(0.0, 0.0, 0.0);
+      public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0);
-      public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(0.0, 0.0, 0.0, 
+      public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0);
-        new TrapezoidProfile.Constraints(0.0, 0.0)
+      public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune
-      );
 
-      public static final double PATH_MAX_VEL = -1; // TODO: find the actual value
+      public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value
-      public static final double PATH_MAX_ACC = -1; // TODO: find the actual value
+      public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
     }
 
     public static final class Conversions {
@@ -106,6 +105,7 @@ public final class Constants {
     }
 
     public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value
+    public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value
 
     // dimensions
     public static final double WIDTH = 18.5;

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

@@ -7,11 +7,32 @@
 
 package frc4388.robot;
 
+import java.util.ArrayList;
+import java.util.List;
+
+import org.opencv.objdetect.HOGDescriptor;
+
+import edu.wpi.first.math.controller.HolonomicDriveController;
+import edu.wpi.first.math.controller.PIDController;
+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.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import edu.wpi.first.math.trajectory.Trajectory.State;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 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.Constants.SwerveDriveConstants.AutoConstants;
+import frc4388.robot.Constants.SwerveDriveConstants.PIDConstants;
 import frc4388.robot.commands.AutoBalance;
 import frc4388.robot.commands.JoystickPlayback;
 import frc4388.robot.commands.JoystickRecorder;
@@ -68,8 +89,9 @@ public class RobotContainer {
         new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
         
-        new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
+        // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive));
+        //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive));
+        //     // .onFalse()
 
         new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
             .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
@@ -94,7 +116,70 @@ public class RobotContainer {
      * @return the command to run in autonomous
      */
     public Command getAutonomousCommand() {
-        return new InstantCommand();
+
+        //Create Trajectory Settings
+        TrajectoryConfig trajectoryConfig = new TrajectoryConfig(SwerveDriveConstants.AutoConstants.PATH_MAX_VEL, 
+            SwerveDriveConstants.AutoConstants.PATH_MAX_ACC).setKinematics(m_robotSwerveDrive.getKinematics());
+
+        // generate trajectory
+        Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
+            
+            new Pose2d(0, 0, new Rotation2d(0)),
+            List.of(
+                    new Translation2d(0, 1)),
+            new Pose2d(0, 2, Rotation2d.fromDegrees(0)),
+            
+            trajectoryConfig);
+
+        ArrayList<Pose2d> simplePath = new ArrayList<Pose2d>();
+        simplePath.add(new Pose2d(0, 0, new Rotation2d(0)));
+        simplePath.add(new Pose2d(0, -1, new Rotation2d(0)));
+
+        // Trajectory trajectory = TrajectoryGenerator.generateTrajectory(simplePath, trajectoryConfig);
+        // Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
+        //     new Pose2d(0, 0, new Rotation2d(0),
+        //     List.of(new Translation2d(0, 1)),
+        //     new Pose2d(0, 2, new Rotation2d(0)),
+        //     trajectoryConfig
+        // );
+
+
+        // ArrayList<double[]> states = new ArrayList<double[]>();
+
+
+
+        //Defining PID Controller for tracking trajectory
+        PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.X_CONTROLLER.kP, 0, 0);
+        PIDController yController = new PIDController(SwerveDriveConstants.AutoConstants.Y_CONTROLLER.kP, 0, 0);
+        ProfiledPIDController thetaController = new ProfiledPIDController(AutoConstants.THETA_CONTROLLER.kP,
+        AutoConstants.THETA_CONTROLLER.kI, AutoConstants.THETA_CONTROLLER.kD, AutoConstants.THETA_CONSTRAINTS);
+        thetaController.enableContinuousInput(-Math.PI, Math.PI);
+
+        HolonomicDriveController holoController = new HolonomicDriveController(xController, yController, thetaController);
+
+        //Command to follow trajectory
+        // SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
+        //     trajectory,
+        //     m_robotSwerveDrive::getOdometry,
+        //     m_robotSwerveDrive.getKinematics(),
+        //     holoController,
+        //     m_robotSwerveDrive::setModuleStates,
+        //     m_robotSwerveDrive);
+
+        SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
+            trajectory,
+            m_robotSwerveDrive::getPoseEstimator,
+            m_robotSwerveDrive.getKinematics(),
+            holoController,
+            m_robotSwerveDrive::setModuleStates,
+            m_robotSwerveDrive);
+
+        //Init and wrap-up, return everything
+        return new SequentialCommandGroup(
+                new InstantCommand(() -> m_robotSwerveDrive.resetPoseEstimator(), m_robotSwerveDrive),
+                // new InstantCommand(() -> m_robotSwerveDrive.setOdometry(trajectory.getInitialPose())),
+                swerveControllerCommand,
+                new InstantCommand(() -> m_robotSwerveDrive.stopModules()));
     }
 
     public DeadbandedXboxController getDeadbandedDriverController() {

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

@@ -5,6 +5,7 @@
 package frc4388.robot.subsystems;
 
 
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -37,7 +38,9 @@ public class SwerveDrive extends SubsystemBase {
 
   private RobotGyro gyro;
 
-  private SwerveDriveOdometry odometry;
+  // private SwerveDriveOdometry odometry;
+
+  private SwerveDrivePoseEstimator poseEstimator;
 
   public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
   public Rotation2d rotTarget = new Rotation2d();
@@ -51,15 +54,28 @@ public class SwerveDrive extends SubsystemBase {
     this.rightBack = rightBack;
     this.gyro = gyro;
 
-    this.odometry = new SwerveDriveOdometry(
+    // this.odometry = new SwerveDriveOdometry(
+    //   kinematics, 
+    //   gyro.getRotation2d(),
+    //   new SwerveModulePosition[] {
+    //     leftFront.getPosition(),
+    //     rightFront.getPosition(),
+    //     leftBack.getPosition(),
+    //     rightBack.getPosition()
+    //   },
+    //   getOdometry()
+    // );
+
+    this.poseEstimator = new SwerveDrivePoseEstimator(
       kinematics, 
-      gyro.getRotation2d(),
+      gyro.getRotation2d(), 
       new SwerveModulePosition[] {
         leftFront.getPosition(),
         rightFront.getPosition(),
         leftBack.getPosition(),
         rightBack.getPosition()
-      }
+      },
+      new Pose2d(0,0, new Rotation2d(0))
     );
 
     this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
@@ -68,21 +84,15 @@ public class SwerveDrive extends SubsystemBase {
 
   public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
     if (fieldRelative) {
-
       if (rightStick.getNorm() > 0.1) {
         rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0, 1));
       }
 
       double rot = rotTarget.minus(gyro.getRotation2d()).getRadians();
 
-
       // Use the left joystick to set speed. Apply a quadratic curve and the set max speed.
       Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
 
-      // if (rightStick.getNorm() < .1) {
-      //   rot = 0;
-      // }
-
       // Convert field-relative speeds to robot-relative speeds.
       chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
 
@@ -112,15 +122,27 @@ public class SwerveDrive extends SubsystemBase {
 
   public void resetGyro() {
     gyro.reset();
-    setOdometry(getOdometry());
+    // setOdometry(getOdometry());
     rotTarget = new Rotation2d(0);
   }
 
   /**
    * Updates the odometry of the SwerveDrive.
    */
-  public void updateOdometry() {
+  // public void updateOdometry() {
-    odometry.update(
+  //   odometry.update(
+  //     gyro.getRotation2d(), 
+  //     new SwerveModulePosition[] {
+  //       leftFront.getPosition(),
+  //       rightFront.getPosition(),
+  //       leftBack.getPosition(),
+  //       rightBack.getPosition()
+  //     }
+  //   );
+  // }
+
+  public void updatePoseEstimator() {
+    poseEstimator.update(
       gyro.getRotation2d(), 
       new SwerveModulePosition[] {
         leftFront.getPosition(),
@@ -135,16 +157,33 @@ public class SwerveDrive extends SubsystemBase {
    * Gets the odometry of the SwerveDrive.
    * @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta).
    */
-  public Pose2d getOdometry() {
+  // public Pose2d getOdometry() {
-    return odometry.getPoseMeters();
+  //   return odometry.getPoseMeters();
+  // }
+
+  public Pose2d getPoseEstimator() {
+    return poseEstimator.getEstimatedPosition();
   }
 
   /**
    * Sets the odometry of the SwerveDrive.
    * @param pose Pose to set the odometry to.
    */
-  public void setOdometry(Pose2d pose) {
+  // public void setOdometry(Pose2d pose) {
-    odometry.resetPosition(
+  //   odometry.resetPosition(
+  //     gyro.getRotation2d(), 
+  //     new SwerveModulePosition[] {
+  //       leftFront.getPosition(),
+  //       rightFront.getPosition(),
+  //       leftBack.getPosition(),
+  //       rightBack.getPosition()
+  //     },
+  //     pose
+  //   );
+  // }
+
+  public void setPoseEstimator(Pose2d pose) {
+    poseEstimator.resetPosition(
       gyro.getRotation2d(), 
       new SwerveModulePosition[] {
         leftFront.getPosition(),
@@ -156,6 +195,10 @@ public class SwerveDrive extends SubsystemBase {
     );
   }
 
+  public void resetPoseEstimator() {
+    setPoseEstimator(new Pose2d());
+  }
+  
   public void stopModules() {
     for (SwerveModule module : this.modules) {
       module.stop();
@@ -166,9 +209,9 @@ public class SwerveDrive extends SubsystemBase {
    * Resets the odometry of the SwerveDrive to 0.
    * *NOTE: If you reset your gyro, this method MUST be called with the new gyro angle and wheel encoder positions.
    */
-  public void resetOdometry() {
+  // public void resetOdometry() {
-    setOdometry(new Pose2d());
+  //   setOdometry(new Pose2d());
-  }
+  // }
 
   public SwerveDriveKinematics getKinematics() {
     return this.kinematics;
@@ -177,14 +220,17 @@ public class SwerveDrive extends SubsystemBase {
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
-    updateOdometry();
 
-    SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX()));
+    // updateOdometry();
-    SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY()));
+    updatePoseEstimator();
-    SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees());
+
+    // SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX()));
+    // SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY()));
+    // SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees());
+
+    // SmartDashboard.putNumber("Gyro Angle", getGyroAngle());
+    // SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees());
 
-    SmartDashboard.putNumber("Gyro Angle", getGyroAngle());
-    SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees());
   }
 
   /**

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

@@ -132,13 +132,13 @@ public class SwerveModule extends SubsystemBase {
         SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
 
         // calculate the difference between our current rotational position and our new rotational position
-        Rotation2d rotationDelta = state.angle.minus(currentRotation); // ? might need to be negative
+        Rotation2d rotationDelta = state.angle.minus(currentRotation);
 
         // calculate the new absolute position of the SwerveModule based on the difference in rotation
         double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
 
         // convert the CANCoder from its position reading to ticks
-        double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); // ? why feedback coefficient
+        double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
 
         if (!ignoreAngle) {
             angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);

src/main/java/frc4388/utility/AprilTag.java

@@ -0,0 +1,13 @@
+package frc4388.utility;
+
+// This is a seperate class in case I want to encode rotation or other
+// information about the tag
+public class AprilTag {
+    public final double x, y, z;
+
+    public AprilTag(double _x, double _y, double _z) {
+        x = _x;
+        y = _y;
+        z = _z;
+    }
+}

src/main/java/frc4388/utility/Gains.java

@@ -54,6 +54,12 @@ public class Gains {
         this.kMinOutput = -1.0;
     }
 
+    public Gains(double kP, double kI, double kD) {
+        this.kP = kP;
+        this.kI = kI;
+        this.kD = kD;
+    }
+
     /**
      * Creates Gains object for PIDs
      * @param kP The P value.