Ofir

.vscode/launch.json

@@ -4,18 +4,24 @@
   // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
   "version": "0.2.0",
   "configurations": [
-
+    {
+      "type": "java",
+      "name": "Launch Main",
+      "request": "launch",
+      "mainClass": "frc.trigon.robot.Main",
+      "projectName": ""
+    },
     {
       "type": "wpilib",
       "name": "WPILib Desktop Debug",
       "request": "launch",
-      "desktop": true,
+      "desktop": true
     },
     {
       "type": "wpilib",
       "name": "WPILib roboRIO Debug",
       "request": "launch",
-      "desktop": false,
+      "desktop": false
     }
   ]
 }

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

@@ -0,0 +1,201 @@
+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.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.*;
+import frc.trigon.robot.utilities.Conversions;
+
+import java.util.Set;
+
+public class Arm extends SubsystemBase {
+    private final static Arm INSTANCE = new Arm();
+
+    private final CANSparkMax
+            masterAngleMotor = ArmConstants.MASTER_ANGLE_MOTOR,
+            followerAngleMotor = ArmConstants.FOLLOWER_ANGLE_MOTOR,
+            masterElevatorMotor = ArmConstants.MASTER_ELEVATOR_MOTOR,
+            followerElevatorMotor = ArmConstants.FOLLOWER_ELEVATOR_MOTOR;
+    private final TalonSRX elevatorEncoder = ArmConstants.ELEVATOR_ENCODER;
+
+    private TrapezoidProfile
+            angleMotorProfile = null,
+            elevatorMotorProfile = null;
+    private double
+            lastAngleProfileGenerationTime,
+            lastElevatorProfileGenerationTime;
+
+    public static Arm getInstance() {
+        return INSTANCE;
+    }
+
+    private Arm() {
+    }
+
+    public Command getSetTargetArmPositionCommand(ArmConstants.ArmState targetState) {
+        return new DeferredCommand(
+                () -> getCurrentSetTargetStateCommand(targetState.elevatorPosition, targetState.angle, 100, 100),
+                Set.of(this)
+        );
+    }
+
+    public Command getSetTargetArmPositionCommand(double elevatorPosition, Rotation2d angle, double angleSpeedPercentage, double elevatorSpeedPercentage) {
+        return new DeferredCommand(
+                () -> getCurrentSetTargetStateCommand(elevatorPosition, angle, angleSpeedPercentage, elevatorSpeedPercentage),
+                Set.of(this)
+        );
+    }
+
+    public Command getSetTargetArmPositionCommand(double elevatorPosition, Rotation2d angle) {
+        return new SequentialCommandGroup(
+                getSetTargetAngleCommand(angle, 100),
+                getSetTargetElevatorPositionCommand(elevatorPosition, 100)
+        );
+    }
+
+    public Command getSetTargetElevatorPositionCommand(double targetElevatorPosition, double speedPercentage) {
+        return new FunctionalCommand(
+                () -> generateElevatorMotorProfile(targetElevatorPosition, speedPercentage),
+                this::setTargetElevatorFromProfile,
+                (interrupted) -> {
+                },
+                () -> false,
+                this
+        );
+    }
+
+    public Command getCurrentSetTargetStateCommand(double elevatorPosition, Rotation2d angle, double angleSpeedPercentage, double elevatorSpeedPercentage) {
+        if (isElevatorOpening(elevatorPosition)) {
+            return new SequentialCommandGroup(
+                    getSetTargetAngleCommand(angle, angleSpeedPercentage),
+                    getSetTargetElevatorPositionCommand(elevatorPosition, elevatorSpeedPercentage)
+            );
+        }
+        return new SequentialCommandGroup(
+                getSetTargetElevatorPositionCommand(elevatorPosition, elevatorSpeedPercentage),
+                getSetTargetAngleCommand(angle, angleSpeedPercentage)
+        );
+    }
+
+    public Command getSetTargetAngleCommand(Rotation2d angle, double anglePercentage) {
+        return new FunctionalCommand(
+                () -> generateAngleMotorProfile(angle, anglePercentage),
+                this::setTargetAngleFromProfile,
+                (interrupted) -> {
+                },
+                () -> false,
+                this
+        );
+    }
+
+    private boolean isElevatorOpening(double targetElevatorPosition) {
+        return targetElevatorPosition > getElevatorPositionRevolutions();
+    }
+
+    private void generateAngleMotorProfile(Rotation2d targetAngle, double speedPercentage) {
+        angleMotorProfile = new TrapezoidProfile(
+                Conversions.scaleConstraints(ArmConstants.ANGLE_CONSTRAINS, speedPercentage),
+                new TrapezoidProfile.State(targetAngle.getDegrees(), 0),
+                new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocityDegreesPerSecond())
+        );
+        lastAngleProfileGenerationTime = Timer.getFPGATimestamp();
+    }
+
+    private void generateElevatorMotorProfile(double targetElevator, double speedPercentage) {
+        elevatorMotorProfile = new TrapezoidProfile(
+                Conversions.scaleConstraints(ArmConstants.ELEVATOR_CONSTRAINS, speedPercentage),
+                new TrapezoidProfile.State(targetElevator, 0),
+                new TrapezoidProfile.State(getElevatorPositionRevolutions(), getElevatorVelocityRevolutionsPerSecond())
+        );
+        lastElevatorProfileGenerationTime = Timer.getFPGATimestamp();
+    }
+
+    private void setTargetAngleFromProfile() {
+        if (angleMotorProfile == null) {
+            stopAngleMotors();
+            return;
+        }
+        TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime());
+        setAngleMotorsVoltage(calculateAngleMotorOutput(targetState));
+    }
+
+    private void setTargetElevatorFromProfile() {
+        if (elevatorMotorProfile == null) {
+            stopElevatorMotors();
+            return;
+        }
+        TrapezoidProfile.State targetState = elevatorMotorProfile.calculate(getElevatorMotorProfileTime());
+        setElevatorMotorsVoltage(calculateElevatorMotorOutput(targetState));
+    }
+
+    private double getAngleMotorProfileTime() {
+        return Timer.getFPGATimestamp() - lastAngleProfileGenerationTime;
+    }
+
+    private double getElevatorMotorProfileTime() {
+        return Timer.getFPGATimestamp() - lastElevatorProfileGenerationTime;
+    }
+
+    private double calculateAngleMotorOutput(TrapezoidProfile.State targetState) {
+        double pidOutput = ArmConstants.ANGLE_PID_CONTROLLER.calculate(
+                getAnglePosition().getDegrees(),
+                targetState.position
+        );
+        double feedforward = ArmConstants.ANGLE_MOTOR_FEEDFORWARD.calculate(
+                Units.degreesToRadians(targetState.position),
+                Units.degreesToRadians(targetState.velocity)
+        );
+        return feedforward + pidOutput;
+    }
+
+    private double calculateElevatorMotorOutput(TrapezoidProfile.State targetState) {
+        double pidOutput = ArmConstants.ELEVATOR_PID_CONTROLLER.calculate(
+                getElevatorPositionRevolutions(),
+                targetState.position
+        );
+        double feedforward = ArmConstants.ELEVATOR_MOTOR_FEEDFORWARD.calculate(targetState.velocity);
+        return feedforward + pidOutput;
+    }
+
+    private double getAngleVelocityDegreesPerSecond() {
+        double positionRevolutions = ArmConstants.ANGLE_MOTOR_VELOCITY_SIGNAL.refresh().getValue();
+        return Conversions.revolutionsToDegrees(positionRevolutions);
+    }
+
+    private double getElevatorVelocityRevolutionsPerSecond() {
+        double magTicksPerSecond = Conversions.perHundredMsToPerSecond(elevatorEncoder.getSelectedSensorVelocity());
+        return Conversions.magTicksToRevolutions(magTicksPerSecond);
+    }
+
+    private Rotation2d getAnglePosition() {
+        double positionRevolutions = ArmConstants.ANGLE_MOTOR_POSITION_SIGNAL.refresh().getValue();
+        return Rotation2d.fromRotations(positionRevolutions);
+    }
+
+    private double getElevatorPositionRevolutions() {
+        return Conversions.magTicksToRevolutions(elevatorEncoder.getSelectedSensorPosition());
+    }
+
+    private void stopAngleMotors() {
+        masterAngleMotor.stopMotor();
+        followerAngleMotor.stopMotor();
+    }
+
+    private void stopElevatorMotors() {
+        masterElevatorMotor.stopMotor();
+        followerElevatorMotor.stopMotor();
+    }
+
+    private void setAngleMotorsVoltage(double voltage) {
+        masterAngleMotor.setVoltage(voltage);
+        followerAngleMotor.setVoltage(voltage);
+    }
+
+    private void setElevatorMotorsVoltage(double voltage) {
+        masterElevatorMotor.setVoltage(voltage);
+        followerElevatorMotor.setVoltage(voltage);
+    }
+}

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

@@ -0,0 +1,157 @@
+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.ElevatorFeedforward;
+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,
+            ELEVATOR_ENCODER_ID = 4,
+            ANGLE_ENCODER_ID = 5;
+
+    private static final double
+            ELEVATOR_ENCODER_OFFSET = 0.0,
+            ANGLE_ENCODER_OFFSET = 0.0;
+
+    private static final boolean
+            MASTER_ANGLE_INVERTED = false,
+            FOLLOWER_ANGLE_INVERTED = false,
+            MASTER_ELEVATOR_INVERTED = false,
+            FOLLOWER_ELEVATOR_INVERTED = true,
+            ELEVATOR_ENCODER_PHASE = true;
+    private static final SensorDirectionValue ANGLE_ENCODER_SENSOR_DIRECTION = SensorDirectionValue.Clockwise_Positive;
+
+    private static final CANSparkMax.IdleMode
+            ANGLE_IDLE_MODE = CANSparkMax.IdleMode.kBrake,
+            ELEVATOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake;
+    private static final AbsoluteSensorRangeValue ANGLE_ENCODER_SENSOR_RANGE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf;
+
+    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);
+    static final TalonSRX ELEVATOR_ENCODER = new TalonSRX(ELEVATOR_ENCODER_ID);
+    static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID);
+
+    private static final double
+            ELEVATOR_P = 1.32,
+            ELEVATOR_I = 3.1,
+            ELEVATOR_D = 0,
+            ANGLE_P = 1.32,
+            ANGLE_I = 3.1,
+            ANGLE_D = 0;
+    static final PIDController
+            ELEVATOR_PID_CONTROLLER = new PIDController(ELEVATOR_P, ELEVATOR_I, ELEVATOR_D),
+            ANGLE_PID_CONTROLLER = new PIDController(ANGLE_P, ANGLE_I, ANGLE_D);
+
+    private static final double
+            ANGLE_MOTOR_KS = 0,
+            ANGLE_MOTOR_KV = 0,
+            ANGLE_MOTOR_KA = 0,
+            ANGLE_MOTOR_KG = 0,
+            ELEVATOR_MOTOR_KS = 0,
+            ELEVATOR_MOTOR_KV = 0,
+            ELEVATOR_MOTOR_KA = 0,
+            ELEVATOR_MOTOR_KG = 0;
+    static final ArmFeedforward ANGLE_MOTOR_FEEDFORWARD = new ArmFeedforward(ANGLE_MOTOR_KS, ANGLE_MOTOR_KG, ANGLE_MOTOR_KV, ANGLE_MOTOR_KA);
+    static final ElevatorFeedforward ELEVATOR_MOTOR_FEEDFORWARD = new ElevatorFeedforward(ELEVATOR_MOTOR_KS, ELEVATOR_MOTOR_KG, ELEVATOR_MOTOR_KV, ELEVATOR_MOTOR_KA);
+
+    private static final double
+            MAX_ANGLE_VELOCITY = 100,
+            MAX_ANGLE_ACCELERATION = 100,
+            MAX_ELEVATOR_VELOCITY = 100,
+            MAX_ELEVATOR_ACCELERATION = 100;
+    static final TrapezoidProfile.Constraints
+            ANGLE_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION),
+            ELEVATOR_CONSTRAINS = new TrapezoidProfile.Constraints(MAX_ELEVATOR_VELOCITY, MAX_ELEVATOR_ACCELERATION);
+
+
+    static final StatusSignal<Double>
+            ANGLE_MOTOR_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(),
+            ANGLE_MOTOR_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity();
+
+    static {
+        configureAngleMotors();
+        configureElevatorMotors();
+        configureElevatorEncoder();
+        configureAngleEncoder();
+    }
+
+    private static void configureElevatorMotors() {
+        MASTER_ELEVATOR_MOTOR.restoreFactoryDefaults();
+        FOLLOWER_ELEVATOR_MOTOR.restoreFactoryDefaults();
+
+        MASTER_ELEVATOR_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION);
+        FOLLOWER_ELEVATOR_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION);
+
+        MASTER_ELEVATOR_MOTOR.setInverted(MASTER_ELEVATOR_INVERTED);
+        FOLLOWER_ELEVATOR_MOTOR.setInverted(FOLLOWER_ELEVATOR_INVERTED);
+
+        MASTER_ELEVATOR_MOTOR.setIdleMode(ELEVATOR_IDLE_MODE);
+        FOLLOWER_ELEVATOR_MOTOR.setIdleMode(ELEVATOR_IDLE_MODE);
+    }
+
+    private static void configureElevatorEncoder() {
+        ELEVATOR_ENCODER.configFactoryDefault();
+        ELEVATOR_ENCODER.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, 0, 10);
+        ELEVATOR_ENCODER.setSelectedSensorPosition(ELEVATOR_ENCODER.getSelectedSensorPosition() - ELEVATOR_ENCODER_OFFSET);
+        ELEVATOR_ENCODER.setSensorPhase(ELEVATOR_ENCODER_PHASE);
+    }
+
+    private static void configureAngleMotors() {
+        MASTER_ANGLE_MOTOR.restoreFactoryDefaults();
+        FOLLOWER_ANGLE_MOTOR.restoreFactoryDefaults();
+
+        MASTER_ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION);
+        FOLLOWER_ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION);
+
+        MASTER_ANGLE_MOTOR.setInverted(MASTER_ANGLE_INVERTED);
+        FOLLOWER_ANGLE_MOTOR.setInverted(FOLLOWER_ANGLE_INVERTED);
+
+        MASTER_ANGLE_MOTOR.setIdleMode(ANGLE_IDLE_MODE);
+        FOLLOWER_ANGLE_MOTOR.setIdleMode(ANGLE_IDLE_MODE);
+    }
+
+    private static void configureAngleEncoder() {
+        CANcoderConfiguration configureAngleEncoder = new CANcoderConfiguration();
+        configureAngleEncoder.MagnetSensor.MagnetOffset = ANGLE_ENCODER_OFFSET;
+        configureAngleEncoder.MagnetSensor.AbsoluteSensorRange = ANGLE_ENCODER_SENSOR_RANGE;
+        configureAngleEncoder.MagnetSensor.SensorDirection = ANGLE_ENCODER_SENSOR_DIRECTION;
+        ANGLE_ENCODER.getConfigurator().apply(configureAngleEncoder);
+        ANGLE_MOTOR_POSITION_SIGNAL.setUpdateFrequency(111);
+        ANGLE_MOTOR_VELOCITY_SIGNAL.setUpdateFrequency(111);
+        ANGLE_ENCODER.optimizeBusUtilization();
+    }
+
+    public enum ArmState {
+        FIRST_STATE(Rotation2d.fromDegrees(30), 1),
+        SECOND_STATE(Rotation2d.fromDegrees(50), 2),
+        THIRD_STATE(Rotation2d.fromDegrees(100), 3);
+
+        final Rotation2d angle;
+        final double elevatorPosition;
+
+        ArmState(Rotation2d angle, double elevatorPosition) {
+            this.elevatorPosition = elevatorPosition;
+            this.angle = angle;
+        }
+    }
+}