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..4285aea05 --- /dev/null +++ b/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(); + } +} \ No newline at end of file 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..ed659c0ab --- /dev/null +++ b/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 + ); + } +} 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..d9e757a45 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -0,0 +1,177 @@ +package frc.trigon.robot.subsystems.arm; + +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.can.WPI_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 int + MASTER_ELEVATOR_MOTOR_ID = 0, + FOLLOWER_ELEVATOR_MOTOR_ID = 0, + MASTER_ANGLE_MOTOR_ID = 0, + FOLLOWER_ANGLE_MOTOR_ID = 0, + ANGLE_ENCODER_ID = 0, + ELEVATOR_ENCODER_ID = 0; + private static final CANSparkMax.IdleMode + ANGLE_IDLE_MODE = CANSparkMax.IdleMode.kBrake, + ELEVATOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; + private static final boolean + MASTER_ANGLE_INVERTED = false, + FOLLOWER_ANGLE_INVERTED = false, + MASTER_ELEVATOR_INVERTED = false, + FOLLOWER_ELEVATOR_INVERTED = false, + ELEVATOR_ENCODER_PHASE = false; + private static final SensorDirectionValue ANGLE_ENCODER_SENSOR_DIRECTION_VALUE = SensorDirectionValue.CounterClockwise_Positive; + private static final double VOLTAGE_COMPENSATION_SATURATION = 12; + private static final double + ANGLE_ENCODER_OFFSET = 0, + ELEVATOR_ENCODER_OFFSET = 0; + private static final AbsoluteSensorRangeValue ANGLE_ENCODER_RANGE_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + static final CANSparkMax + MASTER_ELEVATOR_MOTOR = new CANSparkMax(MASTER_ELEVATOR_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless), + FOLLOWER_ELEVATOR_MOTOR = new CANSparkMax(FOLLOWER_ELEVATOR_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless), + MASTER_ANGLE_MOTOR = new CANSparkMax(MASTER_ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless), + FOLLOWER_ANGLE_MOTOR = new CANSparkMax(FOLLOWER_ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); + static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); + static final WPI_TalonSRX ELEVATOR_ENCODER = new WPI_TalonSRX(ELEVATOR_ENCODER_ID); + + static final double ELEVATOR_METERS_PER_REVOLUTION = 1; + static final double + ANGLE_TOLERANCE_DEGREES = 1, + ELEVATOR_TOLERANCE_METERS = 1; + + private static final double + MAX_ELEVATOR_VELOCITY = 0, + MAX_ELEVATOR_ACCELERATION = 0, + MAX_ANGLE_VELOCITY = 0, + MAX_ANGLE_ACCELERATION = 0; + static final TrapezoidProfile.Constraints ELEVATOR_CONSTRAINTS = new TrapezoidProfile.Constraints( + MAX_ELEVATOR_VELOCITY, + MAX_ELEVATOR_ACCELERATION + ), + ANGLE_CONSTRAINTS = new TrapezoidProfile.Constraints( + MAX_ANGLE_VELOCITY, + MAX_ANGLE_ACCELERATION + ); + + static final StatusSignal + ANGLE_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), + ANGLE_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); + + private static final double + ANGLE_P = 0, + ANGLE_I = 0, + ANGLE_D = 0, + ELEVATOR_P = 0, + 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 + 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_KV, + ANGLE_MOTOR_KA, + ANGLE_MOTOR_KG + ); + static final ElevatorFeedforward ELEVATOR_MOTOR_FEEDFORWARD = new ElevatorFeedforward( + ELEVATOR_MOTOR_KS, + ELEVATOR_MOTOR_KG, + ELEVATOR_MOTOR_KV, + ELEVATOR_MOTOR_KA + ); + + static { + configureAngleMotors(); + configureElevatorMotors(); + configureAngleEncoder(); + configureElevatorEncoder(); + } + + private static void configureAngleMotors() { + MASTER_ANGLE_MOTOR.restoreFactoryDefaults(); + FOLLOWER_ANGLE_MOTOR.restoreFactoryDefaults(); + MASTER_ANGLE_MOTOR.setIdleMode(ANGLE_IDLE_MODE); + FOLLOWER_ANGLE_MOTOR.setIdleMode(ANGLE_IDLE_MODE); + MASTER_ANGLE_MOTOR.setInverted(MASTER_ANGLE_INVERTED); + FOLLOWER_ANGLE_MOTOR.setInverted(FOLLOWER_ANGLE_INVERTED); + MASTER_ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + FOLLOWER_ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + } + + private static void configureElevatorMotors() { + MASTER_ELEVATOR_MOTOR.restoreFactoryDefaults(); + FOLLOWER_ELEVATOR_MOTOR.restoreFactoryDefaults(); + MASTER_ELEVATOR_MOTOR.setIdleMode(ELEVATOR_IDLE_MODE); + FOLLOWER_ELEVATOR_MOTOR.setIdleMode(ELEVATOR_IDLE_MODE); + MASTER_ELEVATOR_MOTOR.setInverted(MASTER_ELEVATOR_INVERTED); + FOLLOWER_ELEVATOR_MOTOR.setInverted(FOLLOWER_ELEVATOR_INVERTED); + MASTER_ELEVATOR_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + FOLLOWER_ELEVATOR_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + } + + private static void configureAngleEncoder() { + CANcoderConfiguration config = new CANcoderConfiguration(); + config.MagnetSensor.MagnetOffset = ANGLE_ENCODER_OFFSET; + config.MagnetSensor.AbsoluteSensorRange = ANGLE_ENCODER_RANGE_VALUE; + config.MagnetSensor.SensorDirection = ANGLE_ENCODER_SENSOR_DIRECTION_VALUE; + ANGLE_ENCODER.getConfigurator().apply(config); + + ANGLE_ENCODER_POSITION_SIGNAL.setUpdateFrequency(100); + ANGLE_ENCODER_VELOCITY_SIGNAL.setUpdateFrequency(100); + ANGLE_ENCODER.optimizeBusUtilization(); + } + + private static void configureElevatorEncoder() { + ELEVATOR_ENCODER.configFactoryDefault(); + ELEVATOR_ENCODER.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute); + ELEVATOR_ENCODER.setSensorPhase(ELEVATOR_ENCODER_PHASE); + ELEVATOR_ENCODER.setSelectedSensorPosition(Conversions.offsetRead(ELEVATOR_ENCODER.getSelectedSensorPosition(), ELEVATOR_ENCODER_OFFSET)); + } + + public enum ArmState { + CONE_COLLECTION(Rotation2d.fromDegrees(0), 0), + CONE_HIGH_STATE(Rotation2d.fromDegrees(0), 0), + CONE_MID_STATE(Rotation2d.fromDegrees(0), 0), + CONE_LOW_STATE(Rotation2d.fromDegrees(0), 0); + + final Rotation2d angle; + final double elevatorPositionMeters; + + ArmState(Rotation2d angle, double elevatorPositionMeters) { + this.angle = angle; + this.elevatorPositionMeters = elevatorPositionMeters; + } + } +} diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java new file mode 100644 index 000000000..d7045d1c0 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -0,0 +1,87 @@ +package frc.trigon.robot.subsystems.sideshooter; + +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +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.subsystems.arm.ArmConstants; +import frc.trigon.robot.utilities.Conversions; + +public class SideShooter extends SubsystemBase { + private final static SideShooter INSTANCE = new SideShooter(); + private final TalonFX shootingMotor = SideShooterConstants.SHOOTING_MOTOR; + private final CANSparkMax angleMotor = SideShooterConstants.ANGLE_MOTOR; + private final VoltageOut shootingVoltageRequest = new VoltageOut(0).withEnableFOC(SideShooterConstants.FOC_ENABLED); + private TrapezoidProfile angleMotorProfile = null; + private double lastAngleMotorProfileGeneration; + + public static SideShooter getInstance() { + return INSTANCE; + } + + private SideShooter() { + } + + void setTargetShootingVoltage(double targetVoltage) { + shootingMotor.setControl(shootingVoltageRequest.withOutput(targetVoltage)); + } + + void generateAngleMotorProfile(Rotation2d targetAngle) { + angleMotorProfile = new TrapezoidProfile( + SideShooterConstants.ANGLE_CONSTRAINTS, + new TrapezoidProfile.State(targetAngle.getDegrees(), 0), + new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocityInDegreesPerSeconds()) + ); + + lastAngleMotorProfileGeneration = Timer.getFPGATimestamp(); + } + + void setTargetAngleFromProfile() { + if (angleMotorProfile == null) { + angleMotor.stopMotor(); + return; + } + + TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleProfileTime()); + angleMotor.setVoltage(calculateAngleOutput(targetState)); + } + + void stopShooting() { + shootingMotor.stopMotor(); + } + + boolean atAngle(Rotation2d angle) { + return Math.abs(angle.getDegrees() - getAnglePosition().getDegrees()) < SideShooterConstants.ANGLE_TOLERANCE; + } + + private double calculateAngleOutput(TrapezoidProfile.State targetState) { + double pidOutput = SideShooterConstants.ANGLE_PID_CONTROLLER.calculate( + getAnglePosition().getDegrees(), + targetState.position + ); + + double feedforward = SideShooterConstants.ANGLE_MOTOR_FEEDFORWARD.calculate( + Units.degreesToRadians(targetState.position), + Units.degreesToRadians(targetState.velocity) + ); + + return pidOutput + feedforward; + } + + private Rotation2d getAnglePosition() { + double positionRevolutions = SideShooterConstants.ANGLE_ENCODER_POSITION_SIGNAL.refresh().getValue(); + return Rotation2d.fromRotations(positionRevolutions); + } + + private double getAngleVelocityInDegreesPerSeconds() { + return Conversions.revolutionsToDegrees(SideShooterConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue()); + } + + private double getAngleProfileTime() { + return Timer.getFPGATimestamp() - lastAngleMotorProfileGeneration; + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java new file mode 100644 index 000000000..e31af97ee --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java @@ -0,0 +1,48 @@ +package frc.trigon.robot.subsystems.sideshooter; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.*; +import frc.trigon.robot.utilities.Commands; + +public class SideShooterCommands { + private static final SideShooter SIDE_SHOOTER = SideShooter.getInstance(); + + /** + * Creates a command that goes to the wanted state. + * + * @param byOrder should the side shooter move parallel commands or sequential commands + * @param targetState the wanted state + * @return the command + */ + public static Command getSetTargetStateCommand(boolean byOrder, SideShooterConstants.SideShooterState targetState) { + if (!byOrder) { + return new ParallelCommandGroup( + Commands.removeRequirements(getSetTargetAngleCommand(targetState.angle)), + getSetTargetShootingVoltageCommand(targetState.voltage) + ); + } + return new SequentialCommandGroup( + getSetTargetAngleCommand(targetState.angle).until(() -> SIDE_SHOOTER.atAngle(targetState.angle)), + getSetTargetShootingVoltageCommand(targetState.voltage) + ); + } + + public static Command getSetTargetAngleCommand(Rotation2d targetAngle) { + return new FunctionalCommand( + () -> SIDE_SHOOTER.generateAngleMotorProfile(targetAngle), + SIDE_SHOOTER::setTargetAngleFromProfile, + (interrupted) -> { + }, + () -> false, + SIDE_SHOOTER + ); + } + + public static Command getSetTargetShootingVoltageCommand(double targetVoltage) { + return new StartEndCommand( + () -> SIDE_SHOOTER.setTargetShootingVoltage(targetVoltage), + SIDE_SHOOTER::stopShooting, + SIDE_SHOOTER + ); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java new file mode 100644 index 000000000..12a1680c9 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -0,0 +1,121 @@ +package frc.trigon.robot.subsystems.sideshooter; + +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +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 SideShooterConstants { + private static final int + SHOOTING_MOTOR_ID = 0, + ANGLE_MOTOR_ID = 0, + ANGLE_ENCODER_ID = 0; + private static final NeutralModeValue SHOOTING_NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; + private static final CANSparkMax.IdleMode ANGLE_IDLE_MODE = CANSparkMax.IdleMode.kBrake; + private static final InvertedValue SHOOTING_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; + private static final boolean ANGLE_INVERTED = false; + private static final SensorDirectionValue ANGLE_ENCODER_SENSOR_DIRECTION_VALUE = SensorDirectionValue.CounterClockwise_Positive; + private static final double ENCODER_OFFSET = 0; + private static final AbsoluteSensorRangeValue ENCODER_RANGE_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + private static final double VOLTAGE_COMPENSATION_SATURATION = 12; + static final boolean FOC_ENABLED = true; + static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); + static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); + static final CANSparkMax ANGLE_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); + static final double ANGLE_TOLERANCE = 1; + + private static final double + MAX_ANGLE_VELOCITY = 0, + MAX_ANGLE_ACCELERATION = 0; + static final TrapezoidProfile.Constraints ANGLE_CONSTRAINTS = new TrapezoidProfile.Constraints( + MAX_ANGLE_VELOCITY, + MAX_ANGLE_ACCELERATION + ); + + static final StatusSignal + ANGLE_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), + ANGLE_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); + + private static final double + ANGLE_P = 0, + ANGLE_I = 0, + ANGLE_D = 0; + static final PIDController 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; + static final ArmFeedforward ANGLE_MOTOR_FEEDFORWARD = new ArmFeedforward( + ANGLE_MOTOR_KS, + ANGLE_MOTOR_KV, + ANGLE_MOTOR_KA, + ANGLE_MOTOR_KG + ); + + static { + configureShootingMotor(); + configureAngleMotor(); + configureAngleEncoder(); + } + + private static void configureShootingMotor() { + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Audio.BeepOnBoot = false; + config.Audio.BeepOnConfig = false; + config.MotorOutput.NeutralMode = SHOOTING_NEUTRAL_MODE_VALUE; + config.MotorOutput.Inverted = SHOOTING_INVERTED_VALUE; + SHOOTING_MOTOR.getConfigurator().apply(config); + SHOOTING_MOTOR.optimizeBusUtilization(); + } + + private static void configureAngleMotor() { + ANGLE_MOTOR.restoreFactoryDefaults(); + ANGLE_MOTOR.setIdleMode(ANGLE_IDLE_MODE); + ANGLE_MOTOR.setInverted(ANGLE_INVERTED); + ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + } + + private static void configureAngleEncoder() { + CANcoderConfiguration config = new CANcoderConfiguration(); + config.MagnetSensor.MagnetOffset = ENCODER_OFFSET; + config.MagnetSensor.AbsoluteSensorRange = ENCODER_RANGE_VALUE; + config.MagnetSensor.SensorDirection = ANGLE_ENCODER_SENSOR_DIRECTION_VALUE; + ANGLE_ENCODER.getConfigurator().apply(config); + + ANGLE_ENCODER_POSITION_SIGNAL.setUpdateFrequency(100); + ANGLE_ENCODER_VELOCITY_SIGNAL.setUpdateFrequency(100); + ANGLE_ENCODER.optimizeBusUtilization(); + } + + public enum SideShooterState { + COLLECTION(Rotation2d.fromDegrees(0), 0), + HIGH_STATE(Rotation2d.fromDegrees(0), 0), + MID_STATE(Rotation2d.fromDegrees(0), 0), + LOW_STATE(Rotation2d.fromDegrees(0), 0); + + final Rotation2d angle; + final double voltage; + + SideShooterState(Rotation2d angle, double voltage) { + this.angle = angle; + this.voltage = voltage; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/utilities/Commands.java b/src/main/java/frc/trigon/robot/utilities/Commands.java index 507bd8d2d..1e4fcce9a 100644 --- a/src/main/java/frc/trigon/robot/utilities/Commands.java +++ b/src/main/java/frc/trigon/robot/utilities/Commands.java @@ -1,10 +1,10 @@ package frc.trigon.robot.utilities; -import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.FunctionalCommand; public class Commands { - public static Command removeRequirements(CommandBase command) { + public static Command removeRequirements(Command command) { return new FunctionalCommand( command::initialize, command::execute,