Miniberty

src/main/java/frc/trigon/robot/subsystems/arm/Arm.java

@@ -0,0 +1,136 @@
+package frc.trigon.robot.subsystems.arm;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.trigon.robot.utilities.Conversions;
+
+public class Arm extends SubsystemBase {
+    private final CANSparkMax
+            MASTER_ELEVATOR_MOTOR = ArmConstants.MASTER_ELEVATOR_MOTOR,
+            FOLLOWER_ELEVATOR_MOTOR = ArmConstants.MASTER_ELEVATOR_MOTOR,
+            MASTER_ANGLE_MOTOR = ArmConstants.MASTER_ELEVATOR_MOTOR,
+            FOLLOWER_ANGLE_MOTOR = ArmConstants.FOLLOWER_ELEVATOR_MOTOR;
+    private final TalonSRX ELEVATOR_TALON_SRX_ENCODER = ArmConstants.ELEVATOR_TALON_SRX_ENCODER;
+    private final static Arm INSTANCE = new Arm();
+
+    private TrapezoidProfile
+            elevatorMotorProfile = null,
+            angleMotorProfile = null;
+    private double lastAngleMotorProfileGenerationTime, lastElevatorMotorProfileGenerationTime;
+
+    public static Arm getInstance() {
+        return INSTANCE;
+    }
+
+    private Arm() {
+    }
+
+    void generateAngleMotorProfile(Rotation2d targetAngle) {
+        angleMotorProfile = new TrapezoidProfile(
+                ArmConstants.ANGLE_CONSTRAINTS,
+                new TrapezoidProfile.State(targetAngle.getDegrees(), 0),
+                new TrapezoidProfile.State(getAngleEncoderPosition().getDegrees(), getAngleVelocityDegreesPerSecond())
+        );
+        lastAngleMotorProfileGenerationTime = Timer.getFPGATimestamp();
+    }
+
+    void setTargetAngleFromProfile() {
+        if (angleMotorProfile == null) {
+            stopAngleMotors();
+            return;
+        }
+        TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime());
+        setTargetAngleVoltage();
+    }
+
+    private void setTargetAngleVoltage(){
+        MASTER_ANGLE_MOTOR.setVoltage(calculateAngleMotorOutput());
+        FOLLOWER_ANGLE_MOTOR.setVoltage(calculateAngleMotorOutput());
+    }
+
+    private double calculateAngleMotorOutput() {
+        TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime());
+        double pidOutput = ArmConstants.ANGLE_PID_CONTROLLER.calculate(
+                getAngleEncoderPosition().getDegrees(),
+                targetState.position
+        );
+        double feedforward = ArmConstants.ANGLE_MOTOR_FEEDFORWARD.calculate(
+                Units.degreesToRadians(targetState.position),
+                Units.degreesToRadians(targetState.velocity)
+        );
+        return pidOutput + feedforward;
+    }
+
+    void generateElevatorMotorProfile(Rotation2d targetElevatorPosition) {
+        elevatorMotorProfile = new TrapezoidProfile(
+                ArmConstants.ELEVATOR_CONSTRAINTS,
+                new TrapezoidProfile.State(targetElevatorPosition.getDegrees(), 0),
+                new TrapezoidProfile.State(getElevatorPositionSecondPerMeter(), getElevatorVelocityRevolutionPerSecond())
+        );
+        lastElevatorMotorProfileGenerationTime = Timer.getFPGATimestamp();
+    }
+
+    void setTargetElevatorFromProfile() {
+        if (elevatorMotorProfile == null) {
+            stopElevatorMotors();
+            return;
+        }
+        setTargetElevatorVoltage();
+    }
+
+    private void setTargetElevatorVoltage(){
+        MASTER_ELEVATOR_MOTOR.setVoltage(calculateElevatorMotorOutput());
+        FOLLOWER_ELEVATOR_MOTOR.setVoltage(calculateElevatorMotorOutput());
+    }
+
+    private double calculateElevatorMotorOutput() {
+        TrapezoidProfile.State targetState = elevatorMotorProfile.calculate(getElevatorMotorProfileTime());
+        double pidOutput = ArmConstants.ELEVATOR_PID_CONTROLLER.calculate(
+                getElevatorPositionSecondPerMeter(),
+                targetState.position
+        );
+        double feedforward = ArmConstants.ELEVATOR_MOTOR_FEEDFORWARD.calculate(
+                Units.degreesToRadians(targetState.position),
+                Units.degreesToRadians(targetState.velocity)
+        );
+        return pidOutput + feedforward;
+    }
+
+    private double getElevatorMotorProfileTime() {
+        return timer.getFPGATimestamp() - lastElevatorMotorProfileGenerationTime;
+    }
+
+    private double getElevatorPositionSecondPerMeter() {
+        return Conversions.perHundredMsToPerSecond(Conversions.magTicksToRevolutions(ArmConstants.METER_PER_REVOLUTION * ELEVATOR_TALON_SRX_ENCODER.getSelectedSensorPosition()));
+    }
+
+    private double getElevatorVelocityRevolutionPerSecond() {
+        return Conversions.perHundredMsToPerSecond(Conversions.magTicksToRevolutions(ELEVATOR_TALON_SRX_ENCODER.getSelectedSensorVelocity()));
+    }
+
+    private double getAngleMotorProfileTime() {
+        return Timer.getFPGATimestamp() - lastAngleMotorProfileGenerationTime;
+    }
+
+    private Rotation2d getAngleEncoderPosition() {
+        return Rotation2d.fromRotations(ArmConstants.ANGEL_ENCODER_POSITION_SIGNAL.refresh().getValue());
+    }
+
+    private double getAngleVelocityDegreesPerSecond() {
+        return Conversions.revolutionsToDegrees(ArmConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue());
+    }
+
+    private void stopAngleMotors() {
+        MASTER_ANGLE_MOTOR.stopMotor();
+        FOLLOWER_ANGLE_MOTOR.stopMotor();
+    }
+
+    private void stopElevatorMotors() {
+        MASTER_ELEVATOR_MOTOR.stopMotor();
+        FOLLOWER_ELEVATOR_MOTOR.stopMotor();
+    }
+}
+

src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java

@@ -0,0 +1,31 @@
+package frc.trigon.robot.subsystems.arm;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+
+public class ArmCommands {
+    private static final Arm ARM = Arm.getInstance();
+
+    public static Command getSetTargetArmAngleCommand(Rotation2d targetAngle) {
+        return new FunctionalCommand(
+                () -> ARM.generateAngleMotorProfile(targetAngle),
+                () -> ARM.setTargetAngleFromProfile(),
+                (interrupted) -> {
+                },
+                () -> false,
+                ARM
+        );
+    }
+
+    public static Command getSetTargetArmElevatorCommand(Rotation2d targetAngle) {
+        return new FunctionalCommand(
+                () -> ARM.generateElevatorMotorProfile(targetAngle),
+                () -> ARM.setTargetElevatorFromProfile(),
+                (interrupted) -> {
+                },
+                () -> false,
+                ARM
+        );
+    }
+}

src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java

@@ -0,0 +1,156 @@
+package frc.trigon.robot.subsystems.arm;
+
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+import com.ctre.phoenix6.hardware.CANcoder;
+import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMaxLowLevel;
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+
+public class ArmConstants {
+
+    private static final double VOLTAGE_COMPENSATION_SATURATION = 12;
+
+    private static final int
+            MASTER_ANGLE_MOTOR_ID = 0,
+            FOLLOWER_ANGLE_MOTOR_ID = 1,
+            MASTER_ELEVATOR_MOTOR_ID = 2,
+            FOLLOWER_ELEVATOR_MOTOR_ID = 3,
+            ANGLE_ENCODER_ID = 4,
+            ELEVATOR_TALON_SRX_ENCODER_ID = 5;
+    static final CANSparkMax
+            MASTER_ANGLE_MOTOR = new CANSparkMax(MASTER_ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless),
+            FOLLOWER_ANGLE_MOTOR = new CANSparkMax(FOLLOWER_ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless),
+            MASTER_ELEVATOR_MOTOR = new CANSparkMax(MASTER_ELEVATOR_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless),
+            FOLLOWER_ELEVATOR_MOTOR = new CANSparkMax(FOLLOWER_ELEVATOR_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless);
+    private static final CANSparkMax.IdleMode
+            MASTER_ANGLE_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake,
+            FOLLOWER_ANGLE_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake,
+            MASTER_ELEVATOR_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake,
+            FOLLOWER_ELEVATOR_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake;
+    private static final boolean
+            MASTER_ANGLE_MOTOR_INVERTED = false,
+            FOLLOWER_ANGLE_MOTOR_INVERTED = false,
+            MASTER_ELEVATOR_MOTOR_INVERTED = false,
+            FOLLOWER_ELEVATOR_MOTOR_INVERTED = true;
+
+    private static final double ANGLE_ENCODER_OFFSET = 0;
+    private static final SensorDirectionValue ANGLE_ENCODER_DIRECTION = SensorDirectionValue.Clockwise_Positive;
+    private static final AbsoluteSensorRangeValue ANGLE_ENCODER_RANGE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf;
+    static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID);
+    static final TalonSRX ELEVATOR_TALON_SRX_ENCODER = new TalonSRX(ELEVATOR_TALON_SRX_ENCODER_ID);
+    private static final double MAG_ENCODER_OFF_SET = 0;
+    static final double METER_PER_REVOLUTION = 1;
+
+    private static final double
+            ANGLE_P = 1,
+            ANGLE_I = 0,
+            ANGLE_D = 0,
+            ELEVATOR_P = 1,
+            ELEVATOR_I = 0,
+            ELEVATOR_D = 0;
+    static final PIDController
+            ANGLE_PID_CONTROLLER = new PIDController(ANGLE_P, ANGLE_I, ANGLE_D),
+            ELEVATOR_PID_CONTROLLER = new PIDController(ELEVATOR_P, ELEVATOR_I, ELEVATOR_D);
+
+    private static final double
+            MAX_ANGLE_VELOCITY = 600,
+            MAX_ANGLE_ACCELERATION = 500,
+            MAX_ELEVATOR_VELOCITY = 600,
+            MAX_ELEVATOR_ACCELERATION = 500;
+    static final TrapezoidProfile.Constraints
+            ANGLE_CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION),
+            ELEVATOR_CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_ELEVATOR_VELOCITY, MAX_ELEVATOR_ACCELERATION);
+    static final StatusSignal<Double> ANGEL_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition();
+    static final StatusSignal<Double> ANGLE_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity();
+
+    private static final double
+            ANGLE_MOTOR_KS = 0.5990,
+            ANGLE_MOTOR_KV = 0.5990,
+            ANGLE_MOTOR_KA = 0.5990,
+            ANGLE_MOTOR_KG = 0.5990,
+            ELEVATOR_MOTOR_KS = 0.5990,
+            ELEVATOR_MOTOR_KV = 0.5990,
+            ELEVATOR_MOTOR_KA = 0.5990,
+            ELEVATOR_MOTOR_KG = 0.5990;
+    static final ArmFeedforward
+            ANGLE_MOTOR_FEEDFORWARD = new ArmFeedforward(ANGLE_MOTOR_KS, ANGLE_MOTOR_KG, ANGLE_MOTOR_KV, ANGLE_MOTOR_KA),
+            ELEVATOR_MOTOR_FEEDFORWARD = new ArmFeedforward(ELEVATOR_MOTOR_KS, ELEVATOR_MOTOR_KG, ELEVATOR_MOTOR_KV, ELEVATOR_MOTOR_KA);
+
+    private static void configureMasterAngleMotor() {
+        MASTER_ANGLE_MOTOR.restoreFactoryDefaults();
+        MASTER_ANGLE_MOTOR.setIdleMode(MASTER_ANGLE_MOTOR_IDLE_MODE);
+        MASTER_ANGLE_MOTOR.setInverted(MASTER_ANGLE_MOTOR_INVERTED);
+        MASTER_ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION);
+    }
+
+    private static void configureFollowerAngleMotor() {
+        FOLLOWER_ANGLE_MOTOR.restoreFactoryDefaults();
+        FOLLOWER_ANGLE_MOTOR.setIdleMode(FOLLOWER_ANGLE_MOTOR_IDLE_MODE);
+        FOLLOWER_ANGLE_MOTOR.setInverted(FOLLOWER_ANGLE_MOTOR_INVERTED);
+        FOLLOWER_ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION);
+    }
+
+    private static void configureMasterElevatorMotor() {
+        MASTER_ELEVATOR_MOTOR.restoreFactoryDefaults();
+        MASTER_ELEVATOR_MOTOR.setIdleMode(MASTER_ELEVATOR_MOTOR_IDLE_MODE);
+        MASTER_ELEVATOR_MOTOR.setInverted(MASTER_ELEVATOR_MOTOR_INVERTED);
+        MASTER_ELEVATOR_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION);
+    }
+
+    private static void configureFollowerElevatorMotor() {
+        FOLLOWER_ELEVATOR_MOTOR.restoreFactoryDefaults();
+        FOLLOWER_ELEVATOR_MOTOR.setIdleMode(FOLLOWER_ELEVATOR_MOTOR_IDLE_MODE);
+        FOLLOWER_ELEVATOR_MOTOR.setInverted(FOLLOWER_ELEVATOR_MOTOR_INVERTED);
+        FOLLOWER_ELEVATOR_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION);
+    }
+
+    private static void configureAngleCanCoder() {
+        CANcoderConfiguration config = new CANcoderConfiguration();
+        config.MagnetSensor.MagnetOffset = ANGLE_ENCODER_OFFSET;
+        config.MagnetSensor.SensorDirection = ANGLE_ENCODER_DIRECTION;
+        config.MagnetSensor.AbsoluteSensorRange = ANGLE_ENCODER_RANGE;
+        ANGLE_ENCODER.getConfigurator().apply(config);
+
+        ANGEL_ENCODER_POSITION_SIGNAL.setUpdateFrequency(100);
+        ANGLE_ENCODER_VELOCITY_SIGNAL.setUpdateFrequency(100);
+        ANGLE_ENCODER.optimizeBusUtilization();
+    }
+
+    private static void configureElevatorMagEncoder() {
+        ELEVATOR_TALON_SRX_ENCODER.configFactoryDefault();
+        ELEVATOR_TALON_SRX_ENCODER.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10);
+        ELEVATOR_TALON_SRX_ENCODER.setSensorPhase(false);
+        ELEVATOR_TALON_SRX_ENCODER.setSelectedSensorPosition(MAG_ENCODER_OFF_SET, 0, 10);
+    }
+
+    static {
+        configureFollowerAngleMotor();
+        configureFollowerElevatorMotor();
+        configureMasterAngleMotor();
+        configureMasterElevatorMotor();
+        configureAngleCanCoder();
+        configureElevatorMagEncoder();
+    }
+
+    public enum ArmState {
+        COLLECTION(Rotation2d.fromDegrees(-15), 0),
+        MIDDLE(Rotation2d.fromDegrees(30), 30),
+        HIGH(Rotation2d.fromDegrees(60), 45);
+
+        final Rotation2d angle;
+        final double elevatorPositionMeters;
+
+        ArmState(Rotation2d angle, double elevatorPositionMeters) {
+            this.angle = angle;
+            this.elevatorPositionMeters = elevatorPositionMeters;
+        }
+    }
+}