vortex changes

grt192 · Jan 31, 2024 · 962289e · 962289e
1 parent cc405f2
commit 962289e
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 9 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -25,7 +25,7 @@ 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;
 

diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java
@@ -47,7 +47,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 = 1 / DRIVE_ROTATIONS_PER_METER;
 
     private static final double STEER_P = .68; // .7
     private static final double STEER_I = 0; // 0
@@ -80,7 +80,7 @@ public SwerveModule(int drivePort, int steerPort, double offsetRads, boolean fal
         // untested for vortexes
         driveMotor.setPositionConversionFactor(DRIVE_ROTATIONS_PER_METER);
         System.out.println("factor " + DRIVE_ROTATIONS_PER_METER);
-        driveMotor.setVelocityConversionFactor(DRIVE_ROTATIONS_PER_METER * 60.0); //Conversion from rpm to m/s
+        driveMotor.setVelocityConversionFactor(DRIVE_ROTATIONS_PER_METER / 60.0); //Conversion from rpm to m/s
 
         steerMotor = new CANSparkMax(steerPort, MotorType.kBrushless);
         // steerMotor.setInverted(true);

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, false);
+        frontRightModule = new SwerveModule(FR_DRIVE, FR_STEER, FR_OFFSET, false);
+        backLeftModule = new SwerveModule(BL_DRIVE, BL_STEER, BL_OFFSET, false);
+        backRightModule = new SwerveModule(BR_DRIVE, BR_STEER, BR_OFFSET, false);
 
         kinematics = new SwerveDriveKinematics(FL_POS, FR_POS, BL_POS, BR_POS);
 

diff --git a/src/main/java/frc/robot/subsystems/swerve/drivemotors/VortexDriveMotor.java b/src/main/java/frc/robot/subsystems/swerve/drivemotors/VortexDriveMotor.java
@@ -16,6 +16,7 @@ public class VortexDriveMotor implements SwerveDriveMotor {
     private SparkPIDController pidController;
 
     public VortexDriveMotor (int port){
+
         motor = new CANSparkMax(port, MotorType.kBrushless);
         motor.setIdleMode(IdleMode.kBrake);
 
@@ -57,11 +58,11 @@ public void setPositionConversionFactor(double factor){
     }
 
     public double getError(){
-        return 0; //STUB
+        return (encoder.getVelocity() - motor.get()); 
     }
 
     public double getSetPoint(){
-        return 0; //STUB
+        return motor.get(); //STUB
     }
 
     public double getAmpDraw(){