Small changes ported over from swerve-base (#14)

* Small changes ported over from swerve-base Includes: * Vision updates to pose estimation * Auto align to a point * Working framework for auton pathing * Some vendordep updates * tested on bot * Pull request edits * Fixed import --------- Co-authored-by: Anay Nagar <[email protected]> Co-authored-by: Anay Nagar <[email protected]>
SaintsRobotics · Feb 21, 2024 · 8bb305a · 8bb305a
1 parent 4e18f68
commit 8bb305a
Show file tree

Hide file tree

Showing 8 changed files with 186 additions and 107 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -48,15 +48,10 @@ public static final class DriveConstants {
     public static final int kFrontRightTurningMotorPort = 37;
     public static final int kRearRightTurningMotorPort = 26;
 
-    public static final int kFrontLeftTurningEncoderPort = 19;
-    public static final int kRearLeftTurningEncoderPort = 20;
-    public static final int kFrontRightTurningEncoderPort = 18;
-    public static final int kRearRightTurningEncoderPort = 17;
-
-    public static final double kFrontLeftTurningEncoderOffset = 0;
-    public static final double kRearLeftTurningEncoderOffset = 0;
-    public static final double kFrontRightTurningEncoderOffset = 0;
-    public static final double kRearRightTurningEncoderOffset = 0;
+    public static final int kFrontLeftTurningEncoderPort = 5;
+    public static final int kRearLeftTurningEncoderPort = 6;
+    public static final int kFrontRightTurningEncoderPort = 3;
+    public static final int kRearRightTurningEncoderPort = 4;
 
     // TODO: Test motor orientations before driving on an actual robot
     public static final boolean kFrontLeftDriveMotorReversed = false;
@@ -78,7 +73,7 @@ public static final class DriveConstants {
     // public static final double kDrivingGearRatio = 6.75; // SDS MK4i's in L2 configuration
 
     // TODO: Tune this PID before running on a robot on the ground
-    public static final double kPModuleTurningController = -0.3;
+    public static final double kPModuleTurningController = 0.3;
 
     public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
         new Translation2d(kWheelBase / 2, kTrackWidth / 2),
@@ -115,7 +110,7 @@ public static final class VisionConstants {
         kCamPose.getRotation().getZ() };
 
     // TODO: Experiment with different std devs in the pose estimator
-    public static final Vector<N3> kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.1);
+    public static final Vector<N3> kOdometrySTDDevs = VecBuilder.fill(0.1, 0.1, 0.01);
     public static final Vector<N3> kVisionSTDDevs = VecBuilder.fill(0.9, 0.9, 0.9);
 
     // Field size in meters

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -4,11 +4,20 @@
 
 package frc.robot;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
+import com.pathplanner.lib.util.PIDConstants;
+import com.pathplanner.lib.util.ReplanningConfig;
+
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.XboxController.Button;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -26,14 +35,26 @@
 public class RobotContainer {
   // The robot's subsystems and commands are defined here
   private final DriveSubsystem m_robotDrive = new DriveSubsystem();
-  public final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem();
-
+  private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem();
+  
   private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort);
 
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
    */
   public RobotContainer() {
+
+    AutoBuilder.configureHolonomic(m_robotDrive::getPose, m_robotDrive::resetOdometry,
+        m_robotDrive::getChassisSpeeds,
+        m_robotDrive::autonDrive,
+        new HolonomicPathFollowerConfig(
+            new PIDConstants(5, 0.0, 0.0), // Translation PID constants
+            new PIDConstants(5, 0.0, 0.0), // Rotation PID constants
+            4.5, // Max module speed, in m/s
+            0.4, // Drive base radius in meters. Distance from robot center to furthest module.
+            new ReplanningConfig(true, true)),
+        () -> false, m_robotDrive);
+
     // Configure the trigger bindings
     configureBindings();
 
@@ -57,7 +78,7 @@ public RobotContainer() {
                         * IOConstants.kSlowModeScalar)
                     * 0.8,
                 MathUtil.applyDeadband(
-                    m_driverController.getRightX(),
+                    -m_driverController.getRightX(),
                     IOConstants.kControllerDeadband)
                     * DriveConstants.kMaxAngularSpeedRadiansPerSecond
                     * (1 - m_driverController
@@ -75,11 +96,15 @@ private void configureBindings() {
     new JoystickButton(m_driverController, Button.kStart.value)
         .onTrue(new InstantCommand(m_robotDrive::zeroHeading, m_robotDrive));
 
+    // new JoystickButton(m_driverController, Button.kA.value).whileTrue(
+    //     AutoBuilder.pathfindToPose(new Pose2d(2.8, 5.5, new Rotation2d()), new PathConstraints(
+    //         DriveConstants.kMaxSpeedMetersPerSecond - 1, 5, DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5)));
+
     // TODO: Move shoot commands to operator controller
-    new JoystickButton(m_driverController, Button.kB.value)
+    new JoystickButton(m_driverController, Button.kX.value)
         .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem))
         .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
-    new JoystickButton(m_driverController, Button.kA.value)
+    new JoystickButton(m_driverController, Button.kY.value)
         .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem))
         .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem));
   }
@@ -90,6 +115,20 @@ private void configureBindings() {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-    return Commands.print("No autonomous command configured");
+    PathPlannerPath path = PathPlannerPath.fromPathFile("New New Path");
+
+    var alliance = DriverStation.getAlliance();
+    PathPlannerPath autonPath = path;
+    if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) {
+      autonPath = autonPath.flipPath();
+    }
+    m_robotDrive.resetOdometry(autonPath.getPreviewStartingHolonomicPose());
+
+    // return AutoBuilder.followPath(autonPath);
+    return null;
+
+    // PathPlannerAuto pathPlannerAuto = new PathPlannerAuto("New Auto");
+
+    // return pathPlannerAuto;
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
@@ -4,6 +4,8 @@
 
 package frc.robot.subsystems;
 
+import java.util.Optional;
+
 import com.kauailabs.navx.frc.AHRS;
 
 import edu.wpi.first.math.MathUtil;
@@ -22,35 +24,32 @@
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.VisionConstants;
 import frc.robot.Robot;
+import frc.robot.subsystems.VisionSubsystem.Measurement;
 
 public class DriveSubsystem extends SubsystemBase {
   private final SwerveModule m_frontLeft = new SwerveModule(
       DriveConstants.kFrontLeftDriveMotorPort,
       DriveConstants.kFrontLeftTurningMotorPort,
       DriveConstants.kFrontLeftTurningEncoderPort,
-      DriveConstants.kFrontLeftDriveMotorReversed,
-      DriveConstants.kFrontLeftTurningEncoderOffset);
+      DriveConstants.kFrontLeftDriveMotorReversed);
 
   private final SwerveModule m_rearLeft = new SwerveModule(
       DriveConstants.kRearLeftDriveMotorPort,
       DriveConstants.kRearLeftTurningMotorPort,
       DriveConstants.kRearLeftTurningEncoderPort,
-      DriveConstants.kRearLeftDriveMotorReversed,
-      DriveConstants.kRearLeftTurningEncoderOffset);
+      DriveConstants.kRearLeftDriveMotorReversed);
 
   private final SwerveModule m_frontRight = new SwerveModule(
       DriveConstants.kFrontRightDriveMotorPort,
       DriveConstants.kFrontRightTurningMotorPort,
       DriveConstants.kFrontRightTurningEncoderPort,
-      DriveConstants.kFrontRightDriveMotorReversed,
-      DriveConstants.kFrontRightTurningEncoderOffset);
+      DriveConstants.kFrontRightDriveMotorReversed);
 
   private final SwerveModule m_rearRight = new SwerveModule(
       DriveConstants.kRearRightDriveMotorPort,
       DriveConstants.kRearRightTurningMotorPort,
       DriveConstants.kRearRightTurningEncoderPort,
-      DriveConstants.kRearRightDriveMotorReversed,
-      DriveConstants.kRearRightTurningEncoderOffset);
+      DriveConstants.kRearRightDriveMotorReversed);
 
   private final AHRS m_gyro = new AHRS();
   private double m_gyroAngle;
@@ -65,10 +64,15 @@ public class DriveSubsystem extends SubsystemBase {
       m_rearRight.getPosition()
   };
 
+  SwerveModuleState[] swerveModuleStates = { new SwerveModuleState(), new SwerveModuleState(), new SwerveModuleState(),
+      new SwerveModuleState() };
+
   private final SwerveDrivePoseEstimator m_poseEstimator = new SwerveDrivePoseEstimator(DriveConstants.kDriveKinematics,
       m_gyro.getRotation2d(), m_swerveModulePositions, new Pose2d(), VisionConstants.kOdometrySTDDevs,
       VisionConstants.kVisionSTDDevs);
 
+  private final VisionSubsystem m_visionSubsystem = new VisionSubsystem();
+
   private final Field2d m_field = new Field2d();
 
   /** Creates a new DriveSubsystem. */
@@ -92,6 +96,14 @@ public void periodic() {
     m_poseEstimator.update(Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle),
         m_swerveModulePositions);
 
+    Optional<Measurement> latestReading = m_visionSubsystem.getMeasurement();
+
+    // SmartDashboard.putBoolean("reading present", latestReading.isPresent());
+
+    if (latestReading.isPresent()) {
+      m_poseEstimator.addVisionMeasurement(latestReading.get().pose.toPose2d(), latestReading.get().timestamp);
+    }
+
     m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
 
     SmartDashboard.putNumber("gyro angle", m_gyro.getAngle());
@@ -106,6 +118,8 @@ public void periodic() {
         m_rearRight.getPosition().angle.getDegrees(), m_rearRight.driveOutput,
     };
     SmartDashboard.putNumberArray("AdvantageScope Swerve States", logData);
+
+    setModuleStates(swerveModuleStates);
   }
 
   /**
@@ -165,13 +179,15 @@ public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRe
 
     // Depending on whether the robot is being driven in field relative, calculate
     // the desired states for each of the modules
-    SwerveModuleState[] swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
+    swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
         fieldRelative
             ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, calculatedRotation,
                 Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle))
             : new ChassisSpeeds(xSpeed, ySpeed, calculatedRotation));
+  }
 
-    setModuleStates(swerveModuleStates);
+  public ChassisSpeeds getChassisSpeeds() {
+    return DriveConstants.kDriveKinematics.toChassisSpeeds(swerveModuleStates);
   }
 
   /**
@@ -202,13 +218,13 @@ public void addVisionMeasurement(Pose2d pose, double timestamp) {
   }
 
   /**
-   * Sets the swerve ModuleStates.
+   * Sets the swerve ModuleStates. Overloaded for either auton builder or teleop.
    *
    * @param desiredStates The desired SwerveModule states.
    */
   public void setModuleStates(SwerveModuleState[] desiredStates) {
-    SwerveDriveKinematics.desaturateWheelSpeeds(
-        desiredStates, DriveConstants.kMaxSpeedMetersPerSecond);
+    SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, DriveConstants.kMaxSpeedMetersPerSecond);
+
     m_frontLeft.setDesiredState(desiredStates[0]);
     m_frontRight.setDesiredState(desiredStates[1]);
     m_rearLeft.setDesiredState(desiredStates[2]);
@@ -229,4 +245,8 @@ public void setModuleStates(SwerveModuleState[] desiredStates) {
         * Robot.kDefaultPeriod;
   }
 
+  public void autonDrive(ChassisSpeeds desiredChassisSpeeds) {
+    swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(desiredChassisSpeeds);
+  }
+
 }
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -4,12 +4,10 @@
 
 package frc.robot.subsystems;
 
-import com.ctre.phoenix6.configs.CANcoderConfigurator;
-import com.ctre.phoenix6.configs.MagnetSensorConfigs;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkFlex;
 import com.revrobotics.CANSparkLowLevel.MotorType;
-import com.revrobotics.CANSparkMax;
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -19,11 +17,10 @@
 import frc.robot.Robot;
 
 public class SwerveModule {
-  private final CANSparkMax m_driveMotor;
-  private final CANSparkMax m_turningMotor;
+  private final CANSparkFlex m_driveMotor;
+  private final CANSparkFlex m_turningMotor;
 
   private final CANcoder m_turningEncoder;
-  private final CANcoderConfigurator m_turningEncoderConfigurator;
 
   private final PIDController m_turningPIDController = new PIDController(DriveConstants.kPModuleTurningController, 0,
       0);
@@ -37,34 +34,33 @@ public class SwerveModule {
   /**
    * Constructs a {@link SwerveModule}.
    *
-   * @param driveMotorPort       The port of the drive motor.
-   * @param turningMotorPort     The port of the turning motor.
-   * @param turningEncoderPort   The port of the turning encoder.
-   * @param driveMotorReversed   Whether the drive motor is reversed.
-   * @param turningEncoderOffset Offset of the turning encoder.
+   * @param driveMotorPort     The port of the drive motor.
+   * @param turningMotorPort   The port of the turning motor.
+   * @param turningEncoderPort The port of the turning encoder.
+   * @param driveMotorReversed Whether the drive motor is reversed.
    */
   public SwerveModule(
       int driveMotorPort,
       int turningMotorPort,
       int turningEncoderPort,
-      boolean driveMotorReversed,
-      double turningEncoderOffset) {
-    m_driveMotor = new CANSparkMax(driveMotorPort, MotorType.kBrushless);
-    m_turningMotor = new CANSparkMax(turningMotorPort, MotorType.kBrushless);
+      boolean driveMotorReversed) {
+    m_driveMotor = new CANSparkFlex(driveMotorPort, MotorType.kBrushless);
+    m_turningMotor = new CANSparkFlex(turningMotorPort, MotorType.kBrushless);
     m_turningEncoder = new CANcoder(turningEncoderPort);
-    m_turningEncoderConfigurator = m_turningEncoder.getConfigurator();
 
     // converts default units to meters per second
     m_driveMotor.getEncoder().setVelocityConversionFactor(
         DriveConstants.kWheelDiameterMeters * Math.PI / 60 / DriveConstants.kDrivingGearRatio);
+    m_driveMotor.getEncoder().setPositionConversionFactor(
+        DriveConstants.kWheelDiameterMeters * Math.PI / DriveConstants.kDrivingGearRatio);
 
     m_driveMotor.setInverted(driveMotorReversed);
 
-    m_turningMotor.setIdleMode(IdleMode.kBrake);
+    m_driveMotor.setIdleMode(IdleMode.kBrake);
+
+    m_turningMotor.setInverted(true);
 
-    // TODO: CANcoder offsets are now set on the device manually using Pheonix Tuner
-    // (or maybe Pheonix X)
-    m_turningEncoderConfigurator.apply(new MagnetSensorConfigs().withMagnetOffset(-turningEncoderOffset));
+    m_turningMotor.setIdleMode(IdleMode.kBrake);
 
     m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI);
   }
@@ -83,7 +79,7 @@ public SwerveModulePosition getPosition() {
     // expected values
     return Robot.isReal()
         ? new SwerveModulePosition(m_driveMotor.getEncoder().getPosition(),
-            getEncoderAngle(m_turningEncoder))
+            getTurnEncoderAngle())
         : new SwerveModulePosition(m_distance, m_state.angle);
   }
 
@@ -93,26 +89,25 @@ public SwerveModulePosition getPosition() {
    * @param desiredState Desired state with speed and angle.
    */
   public void setDesiredState(SwerveModuleState desiredState) {
-    m_state = SwerveModuleState.optimize(desiredState, getEncoderAngle(m_turningEncoder));
+    m_state = SwerveModuleState.optimize(desiredState, getTurnEncoderAngle());
     driveOutput = m_state.speedMetersPerSecond / DriveConstants.kMaxSpeedMetersPerSecond;
 
-    turnOutput = m_turningPIDController.calculate(getEncoderAngle(m_turningEncoder).getRadians(),
+    turnOutput = m_turningPIDController.calculate(getTurnEncoderAngle().getRadians(),
         m_state.angle.getRadians());
 
     m_driveMotor.set(driveOutput);
     m_turningMotor.set(turnOutput);
   }
 
   /**
-   * Returns the angle of a CANcoder
+   * Returns the angle of the turning CANcoder
    * 
    * The CANcoder now gives values in rotations which is useless, so this method
    * translates the CANcoder output into a Rotation2D
    * 
-   * @param encoder The encoder to get the absolute angle of.
    * @return A Rotation2d of the absolute angle.
    */
-  public static Rotation2d getEncoderAngle(CANcoder encoder) {
-    return new Rotation2d(encoder.getAbsolutePosition().getValueAsDouble() * 2 * Math.PI);
+  public Rotation2d getTurnEncoderAngle() {
+    return new Rotation2d(m_turningEncoder.getAbsolutePosition().getValueAsDouble() * 2 * Math.PI);
   }
 }