Worlds, Demo Mode, and Phoenix 6

src/main/deploy/pathplanner/TopGrid3PieceTaxi.path

@@ -24,15 +24,15 @@
     },
     {
       "anchorPoint": {
-        "x": 6.654269729684945,
+        "x": 6.55,
         "y": 4.576056676374828
       },
       "prevControl": {
-        "x": 5.805178456479655,
+        "x": 5.70090872679471,
         "y": 4.597962915155529
       },
       "nextControl": {
-        "x": 5.805178456479655,
+        "x": 5.70090872679471,
         "y": 4.597962915155529
       },
       "holonomicAngle": 180.0,

src/main/java/org/team1540/lib/math/Conversions.java

@@ -3,50 +3,62 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 
 public class Conversions {
+//
+//    /**
+//     * @param positionCounts CANCoder Position Counts
+//     * @param gearRatio Gear Ratio between CANCoder and Mechanism
+//     * @return Degrees of Rotation of Mechanism
+//     */
+//    public static double CANcoderToDegrees(double positionCounts, double gearRatio) {
+//        return positionCounts * (360.0 / (gearRatio * 4096.0));
+//    }
+//
+//    /**
+//     * @param degrees Degrees of rotation of Mechanism
+//     * @param gearRatio Gear Ratio between CANCoder and Mechanism
+//     * @return CANCoder Position Counts
+//     */
+//    public static double degreesToCANcoder(double degrees, double gearRatio) {
+//        return degrees / (360.0 / (gearRatio * 4096.0));
+//    }
+//
+//    /**
+//     * @param counts Falcon Position Counts
+//     * @param gearRatio Gear Ratio between Falcon and Mechanism
+//     * @return Degrees of Rotation of Mechanism
+//     */
+//    public static double falconToDegrees(double positionCounts, double gearRatio) {
+//        return positionCounts * (360.0 / (gearRatio * 2048.0));
+//    }
+//
+//    /**
+//     * @param degrees Degrees of rotation of Mechanism
+//     * @param gearRatio Gear Ratio between Falcon and Mechanism
+//     * @return Falcon Position Counts
+//     */
+//    public static double degreesToFalcon(double degrees, double gearRatio) {
+//        return degrees / (360.0 / (gearRatio * 2048.0));
+//    }
 
-    /**
-     * @param positionCounts CANCoder Position Counts
-     * @param gearRatio Gear Ratio between CANCoder and Mechanism
-     * @return Degrees of Rotation of Mechanism
-     */
-    public static double CANcoderToDegrees(double positionCounts, double gearRatio) {
-        return positionCounts * (360.0 / (gearRatio * 4096.0));
+    public static double degreesToRotations(double degrees, double gearRatio) {
+        return degrees / (360.0 / gearRatio);
     }
-
     /**
-     * @param degrees Degrees of rotation of Mechanism
-     * @param gearRatio Gear Ratio between CANCoder and Mechanism
-     * @return CANCoder Position Counts
-     */
-    public static double degreesToCANcoder(double degrees, double gearRatio) {
-        return degrees / (360.0 / (gearRatio * 4096.0));
-    }
-
-    /**
-     * @param counts Falcon Position Counts
+     * @param rotations The number of rotations on the falcon
      * @param gearRatio Gear Ratio between Falcon and Mechanism
      * @return Degrees of Rotation of Mechanism
      */
-    public static double falconToDegrees(double positionCounts, double gearRatio) {
-        return positionCounts * (360.0 / (gearRatio * 2048.0));
-    }
-
-    /**
-     * @param degrees Degrees of rotation of Mechanism
-     * @param gearRatio Gear Ratio between Falcon and Mechanism
-     * @return Falcon Position Counts
-     */
-    public static double degreesToFalcon(double degrees, double gearRatio) {
-        return degrees / (360.0 / (gearRatio * 2048.0));
+    public static double rotationsToDegrees(double rotations, double gearRatio) {
+        return rotations * (360.0 / gearRatio);
     }
 
     /**
-     * @param velocityCounts Falcon Velocity Counts
+     * @param rotationsPerSecond Falcon Velocity Counts
      * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon RPM)
      * @return RPM of Mechanism
      */
-    public static double falconToRPM(double velocityCounts, double gearRatio) {
-        double motorRPM = velocityCounts * (600.0 / 2048.0);        
+    public static double falconToRPM(double rotationsPerSecond, double gearRatio) {
+        double motorRPM = rotationsPerSecond/60;
         double mechRPM = motorRPM / gearRatio;
         return mechRPM;
     }
@@ -58,53 +70,51 @@ public static double falconToRPM(double velocityCounts, double gearRatio) {
      */
     public static double RPMToFalcon(double RPM, double gearRatio) {
         double motorRPM = RPM * gearRatio;
-        double sensorCounts = motorRPM * (2048.0 / 600.0);
-        return sensorCounts;
+        return motorRPM / 60;
     }
 
     /**
-     * @param velocitycounts Falcon Velocity Counts
+     * @param rotationsPerSecond Falcon Velocity Counts
      * @param circumference Circumference of Wheel
      * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon MPS)
      * @return Falcon Velocity Counts
      */
-    public static double falconToMPS(double velocitycounts, double circumference, double gearRatio){
-        double wheelRPM = falconToRPM(velocitycounts, gearRatio);
+    public static double falconToMPS(double rotationsPerSecond, double circumference, double gearRatio){
+        double wheelRPM = falconToRPM(rotationsPerSecond, gearRatio);
         double wheelMPS = (wheelRPM * circumference) / 60;
         return wheelMPS;
     }
-
+//
     /**
      * @param velocity Velocity MPS
      * @param circumference Circumference of Wheel
      * @param gearRatio Gear Ratio between Falcon and Mechanism (set to 1 for Falcon MPS)
-     * @return Falcon Velocity Counts
+     * @return Falcon RPS
      */
     public static double MPSToFalcon(double velocity, double circumference, double gearRatio){
         double wheelRPM = ((velocity * 60) / circumference);
-        double wheelVelocity = RPMToFalcon(wheelRPM, gearRatio);
-        return wheelVelocity;
+        return RPMToFalcon(wheelRPM, gearRatio);
     }
-
+//
     /**
      * @param positionCounts Falcon Position Counts
      * @param circumference Circumference of Wheel
      * @param gearRatio Gear Ratio between Falcon and Wheel
      * @return Meters
      */
-    public static double falconToMeters(double positionCounts, double circumference, double gearRatio){
-        return positionCounts * (circumference / (gearRatio * 2048.0));
-    }
-
-    /**
-     * @param meters Meters
-     * @param circumference Circumference of Wheel
-     * @param gearRatio Gear Ratio between Falcon and Wheel
-     * @return Falcon Position Counts
-     */
-    public static double MetersToFalcon(double meters, double circumference, double gearRatio){
-        return meters / (circumference / (gearRatio * 2048.0));
+    public static double falconToMeters(double rotations, double circumference, double gearRatio){
+        return rotations * (circumference / gearRatio);
     }
+//
+//    /**
+//     * @param meters Meters
+//     * @param circumference Circumference of Wheel
+//     * @param gearRatio Gear Ratio between Falcon and Wheel
+//     * @return Falcon Position Counts
+//     */
+//    public static double MetersToFalcon(double meters, double circumference, double gearRatio){
+//        return meters / (circumference / (gearRatio * 2048.0));
+//    }
 
     /**
      * @param cartesianAngle angle in the Cartesian angle system

src/main/java/org/team1540/lib/util/COTSFalconSwerveConstants.java

@@ -73,7 +73,7 @@ public static COTSFalconSwerveConstants SDSMK4i(double driveGearRatio){
         /** (150 / 7) : 1 */
         double angleGearRatio = ((150.0 / 7.0) / 1.0);
 
-        double angleKP = 0.3;
+        double angleKP = 0.0600;
         double angleKI = 0.0;
         double angleKD = 0.0;
         double angleKF = 0.0;

src/main/java/org/team1540/robot2023/CTREConfigs.java

@@ -1,54 +1,47 @@
 package org.team1540.robot2023;
 
-import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
-import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
-import com.ctre.phoenix.sensors.AbsoluteSensorRange;
-import com.ctre.phoenix.sensors.CANCoderConfiguration;
-import com.ctre.phoenix.sensors.SensorInitializationStrategy;
-import com.ctre.phoenix.sensors.SensorTimeBase;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
 
 public final class CTREConfigs {
     public TalonFXConfiguration swerveAngleFXConfig;
     public TalonFXConfiguration swerveDriveFXConfig;
-    public CANCoderConfiguration swerveCanCoderConfig;
+    public CANcoderConfiguration swerveCanCoderConfig;
 
     public CTREConfigs(){
         swerveAngleFXConfig = new TalonFXConfiguration();
         swerveDriveFXConfig = new TalonFXConfiguration();
-        swerveCanCoderConfig = new CANCoderConfiguration();
+        swerveCanCoderConfig = new CANcoderConfiguration();
 
         /* Swerve Angle Motor Configurations */
-        SupplyCurrentLimitConfiguration angleSupplyLimit = new SupplyCurrentLimitConfiguration(
-                Constants.Swerve.angleEnableCurrentLimit,
-                Constants.Swerve.angleContinuousCurrentLimit,
-                Constants.Swerve.anglePeakCurrentLimit,
-                Constants.Swerve.anglePeakCurrentDuration);
-
-        swerveAngleFXConfig.slot0.kP = Constants.Swerve.angleKP;
-        swerveAngleFXConfig.slot0.kI = Constants.Swerve.angleKI;
-        swerveAngleFXConfig.slot0.kD = Constants.Swerve.angleKD;
-        swerveAngleFXConfig.slot0.kF = Constants.Swerve.angleKF;
-        swerveAngleFXConfig.supplyCurrLimit = angleSupplyLimit;
+        swerveAngleFXConfig.Slot0.kP = Constants.Swerve.angleKP;
+        swerveAngleFXConfig.Slot0.kI = Constants.Swerve.angleKI;
+        swerveAngleFXConfig.Slot0.kD = Constants.Swerve.angleKD;
+        swerveAngleFXConfig.Slot0.kV = Constants.Swerve.angleKF;
+
+        swerveAngleFXConfig.MotorOutput.NeutralMode = Constants.Swerve.angleNeutralMode;
 
         /* Swerve Drive Motor Configuration */
-        SupplyCurrentLimitConfiguration driveSupplyLimit = new SupplyCurrentLimitConfiguration(
-                Constants.Swerve.driveEnableCurrentLimit,
-                Constants.Swerve.driveContinuousCurrentLimit,
-                Constants.Swerve.drivePeakCurrentLimit,
-                Constants.Swerve.drivePeakCurrentDuration);
-
-        swerveDriveFXConfig.slot0.kP = Constants.Swerve.driveKP;
-        swerveDriveFXConfig.slot0.kI = Constants.Swerve.driveKI;
-        swerveDriveFXConfig.slot0.kD = Constants.Swerve.driveKD;
-        swerveDriveFXConfig.slot0.kF = Constants.Swerve.driveKF;
-        swerveDriveFXConfig.supplyCurrLimit = driveSupplyLimit;
-        swerveDriveFXConfig.openloopRamp = Constants.Swerve.openLoopRamp;
-        swerveDriveFXConfig.closedloopRamp = Constants.Swerve.closedLoopRamp;
+
+
+        swerveDriveFXConfig.Slot0.kP = Constants.Swerve.driveKP;
+        swerveDriveFXConfig.Slot0.kI = Constants.Swerve.driveKI;
+        swerveDriveFXConfig.Slot0.kD = Constants.Swerve.driveKD;
+        swerveDriveFXConfig.Slot0.kV = Constants.Swerve.driveKF;
+
+        swerveDriveFXConfig.MotorOutput.NeutralMode = Constants.Swerve.driveNeutralMode;
+
+        swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimitEnable = Constants.Swerve.driveEnableCurrentLimit;
+        swerveDriveFXConfig.CurrentLimits.SupplyCurrentLimit = Constants.Swerve.driveContinuousCurrentLimit;
+        swerveDriveFXConfig.CurrentLimits.SupplyCurrentThreshold = Constants.Swerve.drivePeakCurrentLimit;
+        swerveDriveFXConfig.CurrentLimits.SupplyTimeThreshold = Constants.Swerve.drivePeakCurrentDuration;
+        swerveDriveFXConfig.OpenLoopRamps.DutyCycleOpenLoopRampPeriod = Constants.Swerve.openLoopRamp;
+        swerveDriveFXConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = Constants.Swerve.closedLoopRamp;
 
         /* Swerve CANCoder Configuration */
-        swerveCanCoderConfig.absoluteSensorRange = AbsoluteSensorRange.Unsigned_0_to_360;
-        swerveCanCoderConfig.sensorDirection = Constants.Swerve.canCoderInvert;
-        swerveCanCoderConfig.initializationStrategy = SensorInitializationStrategy.BootToAbsolutePosition;
-        swerveCanCoderConfig.sensorTimeBase = SensorTimeBase.PerSecond;
+        swerveCanCoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Unsigned_0To1;
+        swerveCanCoderConfig.MagnetSensor.SensorDirection = Constants.Swerve.canCoderInvert ? SensorDirectionValue.Clockwise_Positive : SensorDirectionValue.CounterClockwise_Positive;
     }
 }

src/main/java/org/team1540/robot2023/Constants.java

@@ -1,6 +1,6 @@
 package org.team1540.robot2023;
 
-import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
@@ -122,8 +122,7 @@ public static final class Swerve {
         public static final double angleKF = chosenModule.angleKF;
 
         /* Drive Motor PID Values */
-        public static final double driveKP = 1.1289; //TODO: This must be tuned to specific robot
-//        public static final double driveKP = 0.6;
+        public static final double driveKP = 0.226; //TODO: This must be tuned to specific robot
         public static final double driveKI = 0.0;
         public static final double driveKD = 0.0;
         public static final double driveKF = 0.0;
@@ -145,14 +144,14 @@ public static final class Swerve {
                 Math.hypot(trackWidth / 2.0, wheelBase / 2.0);
 
         /* Neutral Modes */
-        public static final NeutralMode angleNeutralMode = NeutralMode.Coast;
-        public static final NeutralMode driveNeutralMode = NeutralMode.Brake;
+        public static final NeutralModeValue angleNeutralMode = NeutralModeValue.Coast;
+        public static final NeutralModeValue driveNeutralMode = NeutralModeValue.Brake;
 
 
         /* Module Specific Constants */
         /* Front Left Module - Module 0 */
         public static final class Mod0 {
-            private static final int moduleID = 4;
+            private static final int moduleID = 1; //4
             public static final SwerveModuleConstants constants = new SwerveModuleConstants(moduleID, ModuleCorner.FRONT_LEFT);
         }
 
@@ -171,7 +170,7 @@ public static final class Mod2 {
 
         /* Back Right Module - Module 3 */
         public static final class Mod3 {
-            private static final int moduleID = 2;
+            private static final int moduleID = 5;
             public static final SwerveModuleConstants constants = new SwerveModuleConstants(moduleID, ModuleCorner.REAR_RIGHT);
         }
     }
@@ -191,11 +190,11 @@ public static final class ArmConstants {
         public static final int PIGEON_ID = 13;
 
         public static final double PIVOT_FF = 0;
-        public static final double PIVOT_KP = 0.2;
+        public static final double PIVOT_KP = 0.4;
         public static final double PIVOT_KI = 0;
-        public static final double PIVOT_KD = 1;
-        public static final double PIVOT_MAX_ACCEL = 40_000;
-        public static final double PIVOT_CRUISE_SPEED = 20_000;
+        public static final double PIVOT_KD = 0.002;
+        public static final double PIVOT_MAX_ACCEL = 196;
+        public static final double PIVOT_CRUISE_SPEED = 98;
 
         // The distance of the pivot from the ground
         public static final double PIVOT_HEIGHT = 21.5;