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..24fd03869 --- /dev/null +++ b/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(); + } +} + 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..6c16b8201 --- /dev/null +++ b/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 + ); + } +} 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..41797511d --- /dev/null +++ b/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 ANGEL_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(); + static final StatusSignal 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; + } + } +} 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..a8b6d8100 --- /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.CANcoder; +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.wpilibj2.command.SubsystemBase; +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 CANcoder angleEncoder = SideShooterConstants.ANGLE_ENCODER; + + private static final VoltageOut driveVoltageRequest = new VoltageOut(0).withEnableFOC(SideShooterConstants.FOC_ENABLED); + private TrapezoidProfile + angleMotorProfile = null; + private double lastAngleMotorProfileGenerationTime; + + public static SideShooter getInstance() { + return INSTANCE; + } + + private SideShooter() { + } + + boolean atAngle(Rotation2d atAngle){ + return Math.abs(atAngle.getDegrees() - getAnglePosition().getDegrees()) <= 1; + } + + void generateAngleMotorProfile(Rotation2d targetAngle) { + angleMotorProfile = new TrapezoidProfile( + SideShooterConstants.ANGLE_CONSTRAINTS, + new TrapezoidProfile.State(targetAngle.getDegrees(), 0), + new TrapezoidProfile.State(getAnglePosition().getDegrees(), getAngleVelocityDegreesPerSecond()) + ); + lastAngleMotorProfileGenerationTime = Timer.getFPGATimestamp(); + } + + void setTargetAngleFromProfile() { + if (angleMotorProfile == null) { + stopAngleMotor(); + return; + } + + TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); + angleMotor.setVoltage(calculateAngleMotorOutput()); + } + + void setTargetShootingVoltage(double targetVoltage) { + shootingMotor.setControl(driveVoltageRequest.withOutput(targetVoltage)); + } + + void stopAngleMotor() { + angleMotor.stopMotor(); + } + + private double calculateAngleMotorOutput() { + TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); + 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() { + return Rotation2d.fromRotations(SideShooterConstants.ANGLE_ENCODER_POSITION_SIGNAL.refresh().getValue()); + } + + private double getAngleVelocityDegreesPerSecond() { + return Conversions.revolutionsToDegrees(SideShooterConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue()); + } + + private double getAngleMotorProfileTime() { + return Timer.getFPGATimestamp() - lastAngleMotorProfileGenerationTime; + } +} + 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..efeeb31b8 --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java @@ -0,0 +1,41 @@ +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(); + + public static Command getSetTargetStateCommand(Rotation2d targetAngle, double targetVoltage, boolean byOrder) { + if (!byOrder) { + return new ParallelCommandGroup( + Commands.removeRequirements(getSetTargetShooterAngleCommand(targetAngle)), + getSetTargetShootingVoltageCommand(targetVoltage) + ); + } + return new SequentialCommandGroup( + getSetTargetShooterAngleCommand(targetAngle).until(()-> SIDE_SHOOTER.atAngle(Rotation2d.fromRadians(targetAngle.getDegrees()))), + getSetTargetShootingVoltageCommand(targetVoltage) + ); + } + + public static Command getSetTargetShooterAngleCommand(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.stopAngleMotor(), + SIDE_SHOOTER + ); + } +} 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..842938eed --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -0,0 +1,113 @@ +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.controls.VoltageOut; +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 { + static final boolean FOC_ENABLED = true; + private static final double VOLTAGE_COMPENSATION_SATURATION = 12; + private static final int + SHOOTING_MOTOR_ID = 0, + ANGLE_MOTOR_ID = 1, + ANGLE_ENCODER_ID = 2; + private static final double ANGLE_ENCODER_OFFSET = 0; + private static final AbsoluteSensorRangeValue ANGLE_ENCODER_RANGE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + private static final InvertedValue SHOOTING_MOTOR_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; + private static final boolean ANGLE_MOTOR_INVERTED = false; + private static final SensorDirectionValue ANGLE_ENCODER_DIRECTION = SensorDirectionValue.Clockwise_Positive; + private static final NeutralModeValue SHOOTING_NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; + private static final CANSparkMax.IdleMode ANGLE_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; + + static final CANSparkMax ANGLE_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); + static final CANcoder ANGLE_ENCODER = new CANcoder(ANGLE_ENCODER_ID); + static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); + + static final StatusSignal + ANGLE_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), + ANGLE_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); + + private static final double + MAX_ANGLE_VELOCITY = 600, + MAX_ANGLE_ACCELERATION = 500; + static final TrapezoidProfile.Constraints ANGLE_CONSTRAINTS = new TrapezoidProfile.Constraints(MAX_ANGLE_VELOCITY, MAX_ANGLE_ACCELERATION); + + private static final double + ANGLE_MOTOR_P = 1, + ANGLE_MOTOR_I = 0, + ANGLE_MOTOR_D = 0; + static final PIDController ANGLE_PID_CONTROLLER = new PIDController(ANGLE_MOTOR_P, ANGLE_MOTOR_I, ANGLE_MOTOR_D); + + private static final double + ANGLE_MOTOR_KS = 0.5990, + ANGLE_MOTOR_KV = 0.5990, + ANGLE_MOTOR_KA = 0.5990, + ANGLE_MOTOR_KG = 0.5990; + static final ArmFeedforward ANGLE_MOTOR_FEEDFORWARD = new ArmFeedforward( + ANGLE_MOTOR_KS, ANGLE_MOTOR_KG, ANGLE_MOTOR_KV, ANGLE_MOTOR_KA + ); + + static { + configureAngleEncoder(); + configureShootingMotor(); + configureAngleMotor(); + } + + private static void configureShootingMotor() { + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Audio.BeepOnBoot = false; + config.Audio.BeepOnConfig = false; + config.MotorOutput.Inverted = SHOOTING_MOTOR_INVERTED_VALUE; + config.MotorOutput.NeutralMode = SHOOTING_NEUTRAL_MODE_VALUE; + SHOOTING_MOTOR.getConfigurator().apply(config); + + SHOOTING_MOTOR.optimizeBusUtilization(); + } + + private static void configureAngleMotor() { + ANGLE_MOTOR.restoreFactoryDefaults(); + ANGLE_MOTOR.setIdleMode(ANGLE_MOTOR_IDLE_MODE); + ANGLE_MOTOR.setInverted(ANGLE_MOTOR_INVERTED); + ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + } + + private static void configureAngleEncoder() { + CANcoderConfiguration config = new CANcoderConfiguration(); + config.MagnetSensor.AbsoluteSensorRange = ANGLE_ENCODER_RANGE; + config.MagnetSensor.MagnetOffset = ANGLE_ENCODER_OFFSET; + config.MagnetSensor.SensorDirection = ANGLE_ENCODER_DIRECTION; + ANGLE_ENCODER.getConfigurator().apply(config); + + ANGLE_ENCODER_VELOCITY_SIGNAL.setUpdateFrequency(100); + ANGLE_ENCODER_POSITION_SIGNAL.setUpdateFrequency(100); + ANGLE_ENCODER.optimizeBusUtilization(); + } + + public enum SideShooterState { + COLLECTION(Rotation2d.fromDegrees(-15), -5), + MIDDLE(Rotation2d.fromDegrees(30), 5), + HIGH(Rotation2d.fromDegrees(60), 10); + + final Rotation2d angle; + final double voltage; + + SideShooterState(Rotation2d angle, double voltage) { + this.angle = angle; + this.voltage = voltage; + } + } +} +