diff --git a/.vscode/launch.json b/.vscode/launch.json index e6ca564b8..76cb0e93c 100644 --- a/.vscode/launch.json +++ b/.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 } ] } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java new file mode 100644 index 000000000..7705bf168 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -0,0 +1,180 @@ +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() { + } + + 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) + ); + } + + private Command getSetTargetElevatorPositionCommand(double targetElevatorPosition, double speedPercentage) { + return new FunctionalCommand( + () -> generateElevatorMotorProfile(targetElevatorPosition, speedPercentage), + this::setTargetElevatorFromProfile, + (interrupted) -> { + }, + () -> false, + this + ); + } + + private Command getSetTargetAngleCommand(Rotation2d angle, double angleSpeedPercentage) { + return new FunctionalCommand( + () -> generateAngleMotorProfile(angle, angleSpeedPercentage), + 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_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_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); + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java new file mode 100644 index 000000000..2ac792264 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -0,0 +1,33 @@ +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 java.util.Set; + +public class ArmCommands { + private static final Arm ARM = Arm.getInstance(); + + public Command getSetTargetArmStateCommand(ArmConstants.ArmState targetState) { + return getSetTargetArmStateCommand(targetState.elevatorPositionMeters, targetState.angle, 100, 100); + } + + public Command getSetTargetArmStateCommand(ArmConstants.ArmState targetState, double angleSpeedPercentage, double elevatorSpeedPercentage) { + return getSetTargetArmStateCommand(targetState.elevatorPositionMeters, targetState.angle, angleSpeedPercentage, elevatorSpeedPercentage); + } + + public Command getSetTargetArmStateCommand(double elevatorPosition, Rotation2d angle, double angleSpeedPercentage, double elevatorSpeedPercentage) { + return new DeferredCommand( + () -> ARM.getCurrentSetTargetStateCommand(elevatorPosition, angle, angleSpeedPercentage, elevatorSpeedPercentage), + Set.of(ARM) + ); + } + + public Command getSetTargetArmPositionCommand(double elevatorPosition, Rotation2d angle) { + return new DeferredCommand( + () -> ARM.getCurrentSetTargetStateCommand(elevatorPosition, angle, 100, 100), + Set.of(ARM) + ); + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java new file mode 100644 index 000000000..9307d9394 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -0,0 +1,158 @@ +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; +import frc.trigon.robot.utilities.Conversions; + +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 + ANGLE_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), + ANGLE_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(Conversions.offsetRead(ELEVATOR_ENCODER.getSelectedSensorPosition() - ELEVATOR_ENCODER_OFFSET, 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_POSITION_SIGNAL.setUpdateFrequency(111); + ANGLE_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 elevatorPositionMeters; + + ArmState(Rotation2d angle, double elevatorPosition) { + this.elevatorPositionMeters = elevatorPosition; + this.angle = angle; + } + } +}