pivot commands and stuff

src/main/java/frc/robot/Constants.java

@@ -49,6 +49,7 @@ public static final class HardwarePorts {
     public static final int climbLeaderMotor = 3;
     public static final int climbFollowerMotor = 4;
     public static final int pivotMotor = 30;
+    public static final int pivotCANcoderID = 31;
 
   }
 
@@ -78,8 +79,8 @@ public static final class SwerveConstants {
     .SDSMK4i(COTSFalconSwerveConstants.driveGearRatios.SDSMK4_L2);
 
     /* Drivetrain Constants */
-    public static final double trackWidth = 0.5715;
-    public static final double wheelBase = 0.5715;
+    public static final double trackWidth = 0.47625;
+    public static final double wheelBase = 0.47625;
     public static final double wheelCircumference = chosenModule.wheelCircumference;
     /*
     * Swerve Kinematics

src/main/java/frc/robot/commands/SetPivot.java

@@ -0,0 +1,46 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Pivot;
+import frc.robot.subsystems.Pivot.PivotState;;
+
+public class SetPivot extends Command {
+    Pivot s_Pivot;
+    PivotState state;
+    ProfiledPIDController pivotController = new ProfiledPIDController(0.06, 1e-2, 1e-3, new TrapezoidProfile.Constraints(500000, 3000*1e5));
+
+    public SetPivot(PivotState state) {
+        s_Pivot = Pivot.getInstance();
+        this.state = state;
+        addRequirements(s_Pivot);
+    }
+
+    @Override
+    public void initialize() {
+        s_Pivot.setState(state);
+        pivotController.reset(s_Pivot.getCANcoderPosition());
+    }
+
+    @Override
+    public void execute() {
+        double voltage = pivotController.calculate(s_Pivot.getCANcoderPosition(), s_Pivot.getSetPoint());
+        if (Math.abs(s_Pivot.getCANcoderPosition() - s_Pivot.getSetPoint()) < 15) {
+			voltage = 0.7;
+		}
+        s_Pivot.setVoltage(voltage);
+    }
+
+    	@Override
+	public boolean isFinished() {
+		return Math.abs(s_Pivot.getSetPoint() - s_Pivot.getCANcoderPosition()) < 45;
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+		s_Pivot.setVoltage(0);
+		SmartDashboard.putString("eleEnd", "elevator end");
+	}
+}

src/main/java/frc/robot/subsystems/Pivot.java

@@ -1,5 +1,10 @@
 package frc.robot.subsystems;
 
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.configs.MagnetSensorConfigs;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
 import com.revrobotics.CANSparkFlex;
 import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkLowLevel.MotorType;
@@ -16,7 +21,7 @@ public static Pivot getInstance() {
         return instance;
     }
 
-    private enum PivotState {
+    public enum PivotState {
         GROUND(0);
 
         private double pos;
@@ -27,14 +32,31 @@ private PivotState(double position) {
     }
 
     private CANSparkFlex mPivotMotor;
+    private CANcoder pivotCANcoder;
+    private PivotState curState = PivotState.GROUND;
 
     public Pivot() {
         mPivotMotor = new CANSparkFlex(Constants.HardwarePorts.pivotMotor, MotorType.kBrushless);
         configMotor();
+
+        pivotCANcoder = new CANcoder(Constants.HardwarePorts.pivotCANcoderID);
+        configCANcoder();
     }
-
-    public void setPosition(PivotState state) {
-        mPivotMotor.getEncoder().setPosition(state.pos);
+
+    public void setState(PivotState state) {
+        curState = state;
+    }
+
+    public double getCANcoderPosition() {
+        return pivotCANcoder.getPosition().getValueAsDouble();
+    }
+
+    public double getSetPoint() {
+        return curState.pos;
+    }
+
+    public void setVoltage(double voltage) {
+        mPivotMotor.setVoltage(voltage);
     }
 
     private void configMotor() {
@@ -46,4 +68,16 @@ private void configMotor() {
         mPivotMotor.getPIDController().setD(Constants.hardwarePIDs.pivotkD);
     }
 
+    private void configCANcoder(){  
+        /* Swerve CANCoder Configuration */
+        // CANcoder is always initialized to absolute position on boot in Phoenix 6 - https://www.chiefdelphi.com/t/what-kind-of-encoders-are-built-into-the-kraken-motors/447253/7
+
+        MagnetSensorConfigs magnetSensorConfigs = new MagnetSensorConfigs();
+        magnetSensorConfigs.AbsoluteSensorRange = AbsoluteSensorRangeValue.Unsigned_0To1;
+        magnetSensorConfigs.SensorDirection = SensorDirectionValue.CounterClockwise_Positive;
+
+        CANcoderConfiguration swerveCanCoderConfig = new CANcoderConfiguration();
+        swerveCanCoderConfig.MagnetSensor = magnetSensorConfigs;  
+        pivotCANcoder.getConfigurator().apply(swerveCanCoderConfig);
+    }   
 }