swerve update to vortexes

grt192 · Feb 3, 2024 · 24735a8 · 24735a8
1 parent 639a217
commit 24735a8
Show file tree

Hide file tree

Showing 15 changed files with 133 additions and 78 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -48,21 +48,21 @@ public static class TestSingleModuleSwerveConstants {
   }
 
   public static class SwerveConstants {
-    public static final int FL_DRIVE = 0; 
+    public static final int FL_DRIVE = 20; 
     public static final int FL_STEER = 1;
-    public static final double FL_OFFSET = 3.34 - Math.PI / 4;
+    public static final double FL_OFFSET = 3.36 - 1. * Math.PI / 4.;
 
     public static final int FR_DRIVE = 2; 
     public static final int FR_STEER = 3; 
-    public static final double FR_OFFSET = 2.94 - Math.PI * 3 / 4;
+    public static final double FR_OFFSET = 3.02 - Math.PI * 3 / 4;
 
     public static final int BL_DRIVE = 4; 
     public static final int BL_STEER = 5; 
-    public static final double BL_OFFSET = 3.14  + Math.PI / 4;
+    public static final double BL_OFFSET = 2.83  + Math.PI / 4;
 
     public static final int BR_DRIVE = 6; 
     public static final int BR_STEER = 7; 
-    public static final double BR_OFFSET = 2.43 + Math.PI * 3.0 / 4.0;
+    public static final double BR_OFFSET = 2.66 + Math.PI * 3.0 / 4.0;
 
     private static double MODULE_DIST = Units.inchesToMeters(27.25 / 2.0);
     public static final Translation2d FL_POS = new Translation2d(-MODULE_DIST, MODULE_DIST);

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -42,7 +42,7 @@
 
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
-    private final BaseDriveController driveController = new DualJoystickDriveController();
+    private final BaseDriveController driveController = new XboxDriveController();
     private final BaseSwerveSubsystem baseSwerveSubsystem;
 
     private final IntakePivotSubsystem intakePivotSubsystem;
@@ -72,23 +72,23 @@ public class RobotContainer {
     /** The container for the robot. Contains subsystems, OI devices, and commands. */
     public RobotContainer() {
         //construct Test
-        // module = new SwerveModule(6, 7, 0);
+        // module = new SwerveModule(6, 7, 0, true);
         // baseSwerveSubsystem = new TestSingleModuleSwerveSubsystem(module);
-      baseSwerveSubsystem = null;// new SwerveSubsystem();
-      intakePivotSubsystem = new IntakePivotSubsystem();
-      shooterFeederSubsystem = new ShooterFeederSubsystem();
+        baseSwerveSubsystem = new SwerveSubsystem();
+        intakePivotSubsystem = new IntakePivotSubsystem();
+        shooterFeederSubsystem = new ShooterFeederSubsystem();
 
-      shooterPivotSubsystem = new ShooterPivotSubsystem(false);
-      shooterFlywheelSubsystem = new ShooterFlywheelSubsystem();
+        shooterPivotSubsystem = new ShooterPivotSubsystem(false);
+        shooterFlywheelSubsystem = new ShooterFlywheelSubsystem();
 
-      climbSubsystem = new ClimbSubsystem();
+        climbSubsystem = new ClimbSubsystem();
 
-      elevatorSubsystem = new ElevatorSubsystem();
+        elevatorSubsystem = new ElevatorSubsystem();
 
-      traj = Choreo.getTrajectory("Curve");
+        traj = Choreo.getTrajectory("Curve");
 
-    // Configure the trigger bindings
-    configureBindings();
+        // Configure the trigger bindings
+        configureBindings();
   }
 
 
@@ -158,22 +158,22 @@ private void configureBindings() {
 
       } else if(baseSwerveSubsystem instanceof TestSingleModuleSwerveSubsystem){
         final TestSingleModuleSwerveSubsystem testSwerveSubsystem = (TestSingleModuleSwerveSubsystem) baseSwerveSubsystem;
-      // LBumper.onTrue(new InstantCommand(() -> {
-      //   testSwerveSubsystem.decrementTest();
-      //   System.out.println(testSwerveSubsystem.getTest());
-      // }
-      // ));
-
-      // RBumper.onTrue(new InstantCommand(() -> {
-      //   testSwerveSubsystem.incrementTest();
-      //   System.out.println(testSwerveSubsystem.getTest());
-      // }
-      // ));
-
-      // AButton.onTrue(new InstantCommand(() -> {
-      //   testSwerveSubsystem.toggletoRun();
-      //   System.out.println(testSwerveSubsystem.getRunning() ? "Running" : "Not running");
-      // }));
+        driveController.getLeftBumper().onTrue(new InstantCommand(() -> {
+          testSwerveSubsystem.decrementTest();
+          System.out.println(testSwerveSubsystem.getTest());
+        }
+        ));
+
+        driveController.getRightBumper().onTrue(new InstantCommand(() -> {
+          testSwerveSubsystem.incrementTest();
+          System.out.println(testSwerveSubsystem.getTest());
+        }
+        ));
+
+        driveController.getFieldResetButton().onTrue(new InstantCommand(() -> {
+          testSwerveSubsystem.toggletoRun();
+          System.out.println(testSwerveSubsystem.getRunning() ? "Running" : "Not running");
+        }));
 
       } else if (baseSwerveSubsystem instanceof SingleModuleSwerveSubsystem){
         final SingleModuleSwerveSubsystem swerveSubsystem = (SingleModuleSwerveSubsystem) baseSwerveSubsystem;

diff --git a/src/main/java/frc/robot/controllers/BaseDriveController.java b/src/main/java/frc/robot/controllers/BaseDriveController.java
@@ -22,4 +22,8 @@ public abstract class BaseDriveController {
     public abstract double getRotatePower();
 
     public abstract JoystickButton getFieldResetButton();
+
+    public abstract JoystickButton getLeftBumper();
+
+    public abstract JoystickButton getRightBumper();
 }
diff --git a/src/main/java/frc/robot/controllers/DualJoystickDriveController.java b/src/main/java/frc/robot/controllers/DualJoystickDriveController.java
@@ -68,5 +68,13 @@ private double getTurnScaling() {
     public JoystickButton getFieldResetButton() {
         return right2;
     }
+
+    public JoystickButton getRightBumper(){
+        return rightMiddleRightButton;
+    }
+
+    public JoystickButton getLeftBumper(){
+        return rightMiddleLeftButton;
+    }
 
 }
diff --git a/src/main/java/frc/robot/controllers/XboxDriveController.java b/src/main/java/frc/robot/controllers/XboxDriveController.java
@@ -27,7 +27,7 @@ public double getForwardPower() {
 
     @Override
     public double getLeftPower() {
-        return -driveController.getLeftX();
+        return driveController.getLeftX();
     }
 
     @Override
@@ -38,4 +38,12 @@ public double getRotatePower() {
     public JoystickButton getFieldResetButton() {
         return driveAButton;
     }
+
+    public JoystickButton getLeftBumper() {
+        return driveLBumper;
+    }
+
+    public JoystickButton getRightBumper() {
+        return driveRBumper;
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java
@@ -125,9 +125,9 @@ public double getAutoAimAngle(){
     }
 
     public void printCurrentAngle(){
-        System.out.println("radians: " + rotationEncoder.getPosition() + "  degrees: " + rotationEncoder.getPosition() * 57.29);
+        // System.out.println("radians: " + rotationEncoder.getPosition() + "  degrees: " + rotationEncoder.getPosition() * 57.29);
         // System.out.println(pivotMotor.get());
-        System.out.println(rotationPIDController.getFF());
+        // System.out.println(rotationPIDController.getFF());
     }
 
     public double getPosition(){

diff --git a/src/main/java/frc/robot/subsystems/swerve/SingleModuleSwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SingleModuleSwerveSubsystem.java
@@ -3,6 +3,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.Timer;
+import frc.robot.util.Util;
 
 public class SingleModuleSwerveSubsystem extends BaseSwerveSubsystem{
 
@@ -33,13 +34,15 @@ public void setRawPowersWithAngle(double drivePower, double angleRads){
     public void setDrivePowers(double xPower, double yPower){
 
         double velocity = MAX_VEL * Math.sqrt(yPower * yPower + xPower * xPower) / Math.sqrt(2);
-        if(Math.abs(xPower) < .01 && Math.abs(yPower) < .01 ){
-            System.out.println(Math.atan2(0, 0));
-            module.setRawPowers(0, 0);
-            return;
-        }
+        // if(Math.abs(xPower) < .01 && Math.abs(yPower) < .01 ){
+        //     // System.out.println(Math.atan2(0, 0));
+        //     module.setRawPowers(0, 0);
+        //     return;
+        // }
         double angle = Math.atan2(yPower, xPower);
 
+        // System.out.println(velocity);
+
         module.setDesiredState(new SwerveModuleState(velocity, new Rotation2d(angle)));
     }
 
@@ -73,5 +76,10 @@ public void toggletoRun(){
         System.out.println(toRun);
     }
 
+    @Override
+    public void periodic() {
+        System.out.println("error  " + Util.twoDecimals(module.getDriveError()) + " setpoint" + Util.twoDecimals(module.getDriveSetpoint()));
+    }
+
 
 }
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java
@@ -18,6 +18,7 @@
 import frc.robot.subsystems.swerve.drivemotors.SwerveDriveMotor;
 import frc.robot.subsystems.swerve.drivemotors.VortexDriveMotor;
 import frc.robot.util.MotorUtil;
+import frc.robot.util.Util;
 
 public class SwerveModule {
     private final SwerveDriveMotor driveMotor;
@@ -32,8 +33,8 @@ public class SwerveModule {
     private boolean verbose;
 
 
-    private static final double DRIVE_METERS_PER_ROTATION = (13.0 / 90.0) * Math.PI * Units.inchesToMeters(4.0);
-    private static final double DRIVE_ROTATIONS_PER_METER = 1.0 / DRIVE_METERS_PER_ROTATION;
+    private static final double DRIVE_METERS_PER_ROTATION = (13.0 / 90.0) * Math.PI * Units.inchesToMeters(4.0); // .0461
+    private static final double DRIVE_ROTATIONS_PER_METER = 1.0 / DRIVE_METERS_PER_ROTATION; // 21.69
     private static final double STEER_ROTATIONS_PER_RADIAN = (130.0 / 1776.0) * 2.0 * Math.PI; // Useful for steer relative encoder if we ever use that
     private static final double STEER_VOLTS_RADIANS = 2 * Math.PI / 3.3 ; // https://docs.revrobotics.com/sparkmax/feature-description/data-port#analog-input
     //The encoder board maps the 5V output of the encoder to 3.3V of the Spark Max
@@ -47,7 +48,7 @@ public class SwerveModule {
     private static final double VORTEX_DRIVE_P = 0;
     private static final double VORTEX_DRIVE_I = 0;
     private static final double VORTEX_DRIVE_D = 0;
-    private static final double VORTEX_DRIVE_FF = 0;
+    private static final double VORTEX_DRIVE_FF = .22591262  * 3.6 / 4; // rotations/m * max vel
 
     private static final double STEER_P = .68; // .7
     private static final double STEER_I = 0; // 0
@@ -64,17 +65,17 @@ public class SwerveModule {
      * @param drivePort The CAN ID of the drive motor
      * @param steerPort The CAN ID of the steer motor
      * @param offsetRads The offset of the absolute encoder
-     * @param falcon Whether this is a falcon or not
+     * @param vortex Whether this is a falcon or not
      */
 
-    public SwerveModule(int drivePort, int steerPort, double offsetRads, boolean falcon) {
+    public SwerveModule(int drivePort, int steerPort, double offsetRads, boolean vortex) {
 
-        driveMotor = falcon ? new FalconDriveMotor(drivePort) : new VortexDriveMotor(drivePort);
+        driveMotor = vortex ? new VortexDriveMotor(drivePort) : new FalconDriveMotor(drivePort);
 
-        if(falcon){
-            driveMotor.configPID(FALCON_DRIVE_P, FALCON_DRIVE_I, FALCON_DRIVE_D, FALCON_DRIVE_FF);
-        } else {
+        if(vortex){
             driveMotor.configPID(VORTEX_DRIVE_P, VORTEX_DRIVE_I, VORTEX_DRIVE_D, VORTEX_DRIVE_FF);
+        } else {
+            driveMotor.configPID(FALCON_DRIVE_P, FALCON_DRIVE_I, FALCON_DRIVE_D, FALCON_DRIVE_FF);
         }
 
         // untested for vortexes
@@ -159,7 +160,7 @@ public void setDesiredState(SwerveModuleState state) {
 
     public void setVerbose(){
         if (crimor.advanceIfElapsed(.1)){
-            System.out.println(" current " + twoDecimals(getWrappedAngle().getDegrees()));
+            System.out.println(" current " + Util.twoDecimals(getWrappedAngle().getDegrees()));
             // System.out.println(" target " + twoDecimals(Math.toDegrees(MathUtil.angleModulus(targetAngleRads))));
             // System.out.print(" error " + twoDecimals(driveMotor.getError()));
             // System.out.println(" target " + twoDecimals(driveMotor.getSetPoint()));
@@ -190,7 +191,7 @@ public void setRawPowersWithAngle(double drivePower, double angleRads){
         double targetAngleRads = angleRads - offsetRads;
 
         driveMotor.setPower(drivePower);
-        System.out.println(Math.abs(currentAngle.minus(new Rotation2d(targetAngleRads)).getDegrees()));
+        // System.out.println(Math.abs(currentAngle.minus(new Rotation2d(targetAngleRads)).getDegrees()));
         //System.out.print("target " + new Rotation2d(targetAngleRads).getDegrees()  + "--------");
         // System.out.print("error " + (angleRads.minus(currentAngle).getRadians()) + "--------");
 
@@ -228,15 +229,19 @@ public Rotation2d getRawAngle(){
         return new Rotation2d(steerAbsoluteEncoder.getPosition());
     }
 
-    public double twoDecimals(double num){
-        return ((int) (num * 100)) / 100.d;
-    }
-
     public double getDriveAmpDraws(){
         return driveMotor.getAmpDraw();
     }
 
     public double getSteerAmpDraws(){
         return steerMotor.getOutputCurrent();
     }
+
+    public double getDriveError(){
+        return driveMotor.getError();
+    }
+
+    public double getDriveSetpoint(){
+        return driveMotor.getSetpoint();
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -77,10 +77,10 @@ public class SwerveSubsystem extends BaseSwerveSubsystem{
     public SwerveSubsystem() {
         ahrs = new AHRS(SPI.Port.kMXP);
 
-        frontLeftModule = new SwerveModule(FL_DRIVE, FL_STEER, FL_OFFSET);
-        frontRightModule = new SwerveModule(FR_DRIVE, FR_STEER, FR_OFFSET);
-        backLeftModule = new SwerveModule(BL_DRIVE, BL_STEER, BL_OFFSET);
-        backRightModule = new SwerveModule(BR_DRIVE, BR_STEER, BR_OFFSET);
+        frontLeftModule = new SwerveModule(FL_DRIVE, FL_STEER, FL_OFFSET, true);
+        frontRightModule = new SwerveModule(FR_DRIVE, FR_STEER, FR_OFFSET, true);
+        backLeftModule = new SwerveModule(BL_DRIVE, BL_STEER, BL_OFFSET, true);
+        backRightModule = new SwerveModule(BR_DRIVE, BR_STEER, BR_OFFSET, true);
 
         kinematics = new SwerveDriveKinematics(FL_POS, FR_POS, BL_POS, BR_POS);
 
@@ -120,6 +120,8 @@ public SwerveSubsystem() {
     }
 
     public void periodic() {
+
+        // System.out.println(frontLeftModule.getDriveSetpoint());
 
         // if (crimer.advanceIfElapsed(.1)){
         //     //System.out.println("BR : " + backRightModule.getRawAngle());
@@ -155,6 +157,7 @@ public void periodic() {
         frontRightModule.setDesiredState(states[1]);
         backLeftModule.setDesiredState(states[2]);
         backRightModule.setDesiredState(states[3]);
+
     }
 
     public void setDrivePowers(double xPower, double yPower, double angularPower){

diff --git a/src/main/java/frc/robot/subsystems/swerve/TestSingleModuleSwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/TestSingleModuleSwerveSubsystem.java
@@ -5,12 +5,13 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
+import frc.robot.util.Util;
 
 public class TestSingleModuleSwerveSubsystem extends SingleModuleSwerveSubsystem {
 
 
     public static final double STEER_POWER = .4;
-    public static final double DRIVE_POWER = .3;
+    public static final double DRIVE_POWER = 1;
 
     private int testCase;
     private boolean toRun;
@@ -138,12 +139,8 @@ public void periodic() {
             super.setRawPowersWithAngle(drive, steer);
         }
         else{
-            System.out.println(drive);
             super.setRawPowers(drive, steer);
         }
-
-
-        // System.out.println(testCase);
     }
 
     public void incrementTest(){
@@ -164,10 +161,6 @@ public boolean getRunning(){
         return toRun;
     }
 
-    public double twoDecimals(double num){
-        return ((int) (num * 100)) / 100.d;
-    }
-
     public void toggletoRun(){
         toRun = !toRun;
         System.out.println(toRun);

diff --git a/src/main/java/frc/robot/subsystems/swerve/drivemotors/FalconDriveMotor.java b/src/main/java/frc/robot/subsystems/swerve/drivemotors/FalconDriveMotor.java
@@ -60,7 +60,7 @@ public double getError(){
         return motor.getClosedLoopError().getValue();
     }
 
-    public double getSetPoint(){
+    public double getSetpoint(){
         return targetRps;
     }
 

diff --git a/src/main/java/frc/robot/subsystems/swerve/drivemotors/SwerveDriveMotor.java b/src/main/java/frc/robot/subsystems/swerve/drivemotors/SwerveDriveMotor.java
@@ -18,7 +18,7 @@ public interface  SwerveDriveMotor {
 
     public double getError();
 
-    public double getSetPoint();
+    public double getSetpoint();
 
     public double getAmpDraw();
 }