Yishay Amir

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

@@ -0,0 +1,162 @@
+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.SubsystemBase;
+import frc.trigon.robot.utilities.Conversions;
+
+public class Arm extends SubsystemBase {
+    private final static Arm INSTANCE = new Arm();
+    private final CANSparkMax
+            masterElevatorMotor = ArmConstants.MASTER_ELEVATOR_MOTOR,
+            followerElevatorMotor = ArmConstants.FOLLOWER_ELEVATOR_MOTOR,
+            masterAngleMotor = ArmConstants.MASTER_ELEVATOR_MOTOR,
+            followerAngleMotor = ArmConstants.FOLLOWER_ANGLE_MOTOR;
+    private final TalonSRX elevatorEncoder = ArmConstants.ELEVATOR_ENCODER;
+    private TrapezoidProfile
+            angleMotorProfile = null,
+            elevatorMotorProfile = null;
+    private double
+            lastAngleMotorProfileGeneration,
+            lastElevatorMotorProfileGeneration;
+
+    public static Arm getInstance() {
+        return INSTANCE;
+    }
+
+    private Arm() {
+    }
+
+    void generateElevatorMotorProfile(double targetPositionMeters, double speedPercentage) {
+        elevatorMotorProfile = new TrapezoidProfile(
+                Conversions.scaleConstraints(ArmConstants.ELEVATOR_CONSTRAINTS, speedPercentage),
+                new TrapezoidProfile.State(targetPositionMeters, 0),
+                new TrapezoidProfile.State(getElevatorPositionRevolutions(), getElevatorVelocityMetersPerSeconds())
+        );
+
+        lastElevatorMotorProfileGeneration = Timer.getFPGATimestamp();
+    }
+
+    void generateAngleMotorProfile(Rotation2d targetAngle, double speedPercentage) {
+        angleMotorProfile = new TrapezoidProfile(
+                Conversions.scaleConstraints(ArmConstants.ANGLE_CONSTRAINTS, speedPercentage),
+                new TrapezoidProfile.State(targetAngle.getDegrees(), 0),
+                new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocityDegreesPerSeconds())
+        );
+
+        lastAngleMotorProfileGeneration = Timer.getFPGATimestamp();
+    }
+
+    void setTargetAngleFromProfile() {
+        if (angleMotorProfile == null) {
+            stopAngleMotors();
+            return;
+        }
+
+        TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleProfileTime());
+        setAngleMotorsVoltage(calculateAngleOutput(targetState));
+    }
+
+    void setTargetElevatorPositionFromProfile() {
+        if (angleMotorProfile == null) {
+            stopElevatorMotors();
+            return;
+        }
+
+        TrapezoidProfile.State targetState = elevatorMotorProfile.calculate(getElevatorProfileTime());
+        setElevatorMotorsVoltage(calculateElevatorOutput(targetState));
+    }
+
+    boolean atAngle(Rotation2d angle) {
+        return Math.abs(angle.getDegrees() - getAnglePosition().getDegrees()) < ArmConstants.ANGLE_TOLERANCE_DEGREES;
+    }
+
+    boolean atTargetPositionElevator(double positionMeters) {
+        return Math.abs(positionMeters - getElevatorPositionRevolutions()) < ArmConstants.ELEVATOR_TOLERANCE_METERS;
+    }
+
+    boolean isElevatorOpening(double targetMeters) {
+        return targetMeters > getElevatorPositionRevolutions();
+    }
+
+    private double getElevatorVelocityMetersPerSeconds() {
+        double elevatorVelocityMagTicksPer100ms = elevatorEncoder.getSelectedSensorVelocity();
+        double elevatorVelocityMagTicksPerSecond = Conversions.perHundredMsToPerSecond(elevatorVelocityMagTicksPer100ms);
+        double elevatorVelocityRevolutionPerSecond = Conversions.magTicksToRevolutions(elevatorVelocityMagTicksPerSecond);
+        return elevatorVelocityRevolutionPerSecond / ArmConstants.ELEVATOR_METERS_PER_REVOLUTION;
+    }
+
+    private Rotation2d getAnglePosition() {
+        double positionRevolutions = ArmConstants.ANGLE_ENCODER_POSITION_SIGNAL.refresh().getValue();
+        return Rotation2d.fromRotations(positionRevolutions);
+    }
+
+    private double getElevatorPositionRevolutions() {
+        double elevatorPositionMagTicks = elevatorEncoder.getSelectedSensorPosition();
+        double elevatorPositionRevolution = Conversions.magTicksToRevolutions(elevatorPositionMagTicks);
+        return elevatorPositionRevolution / ArmConstants.ELEVATOR_METERS_PER_REVOLUTION;
+    }
+
+    private double getAngleVelocityDegreesPerSeconds() {
+        return Conversions.revolutionsToDegrees(ArmConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue());
+    }
+
+    private double calculateElevatorOutput(TrapezoidProfile.State targetState) {
+        double pidOutput = ArmConstants.ELEVATOR_PID_CONTROLLER.calculate(
+                getElevatorPositionRevolutions(),
+                targetState.position
+        );
+
+        double feedforward = ArmConstants.ELEVATOR_MOTOR_FEEDFORWARD.calculate(
+                targetState.velocity
+        );
+
+        return pidOutput + feedforward;
+    }
+
+    private double calculateAngleOutput(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 pidOutput + feedforward;
+    }
+
+    private void setAngleMotorsVoltage(double outputVoltage) {
+        masterAngleMotor.setVoltage(outputVoltage);
+        followerAngleMotor.setVoltage(outputVoltage);
+    }
+
+    private void setElevatorMotorsVoltage(double outputVoltage) {
+        masterElevatorMotor.setVoltage(outputVoltage);
+        followerElevatorMotor.setVoltage(outputVoltage);
+    }
+
+    private double getAngleProfileTime() {
+        return Timer.getFPGATimestamp() - lastAngleMotorProfileGeneration;
+    }
+
+    private double getElevatorProfileTime() {
+        return Timer.getFPGATimestamp() - lastElevatorMotorProfileGeneration;
+    }
+
+    private void stopElevatorMotors() {
+        masterElevatorMotor.stopMotor();
+        followerElevatorMotor.stopMotor();
+    }
+
+    private void stopAngleMotors() {
+        masterAngleMotor.stopMotor();
+        followerAngleMotor.stopMotor();
+    }
+}

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

@@ -0,0 +1,81 @@
+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.DeferredCommand;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+import java.util.Set;
+
+public class ArmCommands {
+    private static final Arm ARM = Arm.getInstance();
+
+    public static Command getSetTargetStateCommand(ArmConstants.ArmState state) {
+        return getSetTargetPositionCommand(state.angle, state.elevatorPositionMeters, 100, 100);
+    }
+
+    /**
+     * set the arm to the wanted state
+     *
+     * @param state                   the wanted state
+     * @param elevatorSpeedPercentage max speed of the elevator in percentages
+     * @param angleSpeedPercentage    max speed of the elevator angle in percentages
+     * @return a command
+     */
+    public static Command getsetTargetStateCommand(ArmConstants.ArmState state, double elevatorSpeedPercentage, double angleSpeedPercentage) {
+        return getSetTargetPositionCommand(state.angle, state.elevatorPositionMeters, elevatorSpeedPercentage, angleSpeedPercentage);
+    }
+
+    public static Command getSetTargetPositionCommand(Rotation2d targetAngle, double targetElevatorPositionMeters) {
+        return getSetTargetPositionCommand(targetAngle, targetElevatorPositionMeters, 100, 100);
+    }
+
+    /**
+     * set the arm to the wanted position
+     *
+     * @param targetAngle                  the target angle
+     * @param targetElevatorPositionMeters the target of the elevator
+     * @param elevatorSpeedPercentage      max speed of the elevator in percentages
+     * @param angleSpeedPercentage         max speed of the elevator angle in percentages
+     * @return a command
+     */
+    public static Command getSetTargetPositionCommand(Rotation2d targetAngle, double targetElevatorPositionMeters, double elevatorSpeedPercentage, double angleSpeedPercentage) {
+        return new DeferredCommand(() -> getCurrentSetTargetPositionCommand(targetAngle, targetElevatorPositionMeters, elevatorSpeedPercentage, angleSpeedPercentage), Set.of());
+    }
+
+    private static Command getCurrentSetTargetPositionCommand(Rotation2d targetAngle, double targetElevatorPositionMeters, double elevatorSpeedPercentage, double angleSpeedPercentage) {
+        if (ARM.isElevatorOpening(targetElevatorPositionMeters)) {
+            return new SequentialCommandGroup(
+                    getSetTargetAngleCommand(targetAngle, angleSpeedPercentage).until(() -> ARM.atAngle(targetAngle)),
+                    getSetTargetElevatorPositionCommand(targetElevatorPositionMeters, elevatorSpeedPercentage)
+            );
+        }
+        return new SequentialCommandGroup(
+                getSetTargetElevatorPositionCommand(targetElevatorPositionMeters, elevatorSpeedPercentage).until(() -> ARM.atTargetPositionElevator(targetElevatorPositionMeters)),
+                getSetTargetAngleCommand(targetAngle, angleSpeedPercentage)
+        );
+    }
+
+    private static Command getSetTargetAngleCommand(Rotation2d targetAngle, double SpeedPercentage) {
+        return new FunctionalCommand(
+                () -> ARM.generateAngleMotorProfile(targetAngle, SpeedPercentage),
+                ARM::setTargetAngleFromProfile,
+                (interrupted) -> {
+                },
+                () -> false,
+                ARM
+        );
+    }
+
+    private static Command getSetTargetElevatorPositionCommand(double targetElevatorPositionMeters, double SpeedPercentage) {
+        return new FunctionalCommand(
+                () -> ARM.generateElevatorMotorProfile(targetElevatorPositionMeters, SpeedPercentage),
+                ARM::setTargetElevatorPositionFromProfile,
+                (interrupted) -> {
+                },
+                () -> false,
+                ARM
+        );
+    }
+}