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..17df1fdcc --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java @@ -0,0 +1,67 @@ +package frc.trigon.robot.subsystems.sideshooter; + + +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.utilities.Conversions; + +public class SideShooter extends SubsystemBase { + private final TalonFX shootingMotor = SideShooterConstants.SHOOTING_MOTOR; + private final CANSparkMax angleMotor = SideShooterConstants.ANGLE_MOTOR; + private TrapezoidProfile angleMotorProfile = null; + private double lastAngleMotorProfileGenerationTime; + + private final static SideShooter INSTANCE = new SideShooter(); + + public static SideShooter getInstance() { + return INSTANCE; + } + + double getAnglePosition() { + double positionRevolutions = SideShooterConstants.ANGLE_ENCODER_POSITION_SIGNAL.refresh().getValue(); + return positionRevolutions; + } + + Double getAngleVelocityDegreesPerSecond() { + return Conversions.revolutionsToDegrees(SideShooterConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue()); + } + + double getAngleMotorProfileTime() { + return Timer.getFPGATimestamp() - lastAngleMotorProfileGenerationTime; + } + + void generateAngleMotorProfile(Rotation2d targetAngle) { + angleMotorProfile = new TrapezoidProfile(SideShooterConstants.ANGLE_Constraints, + new TrapezoidProfile.State(targetAngle.getDegrees(), 0), + new TrapezoidProfile.State(getAnglePosition(), getAngleVelocityDegreesPerSecond())); + + lastAngleMotorProfileGenerationTime = Timer.getFPGATimestamp(); + } + + double calculateAngleOutput(TrapezoidProfile.State targetState) { + double pidOutPut = SideShooterConstants.ANGLE_PID_CONTROLLER.calculate( + getAnglePosition(), + targetState.position + ); + double feedforward = SideShooterConstants.SIDE_SHOOTER_FEEDFORWARD.calculate( + Units.degreesToRadians(targetState.position), + targetState.velocity + ); + angleMotor.setVoltage(pidOutPut = feedforward); + return feedforward + pidOutPut; + } + + void setTargetAngleMotorProfileTime() { + if (angleMotorProfile == null) { + angleMotor.stopMotor(); + return; + } + TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime()); + angleMotor.setVoltage(calculateAngleOutput(targetState)); + } +} 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..d4fbe036d --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterCommands.java @@ -0,0 +1,23 @@ +package frc.trigon.robot.subsystems.sideshooter; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.FunctionalCommand; + +public class SideShooterCommands { + + public static SideShooterCommands getInstance() { + return getInstance(); + } + + + public Command getSetTargetShooterAngleCommand(Rotation2d targetAngle) { + return new FunctionalCommand( + () -> SideShooter.getInstance().generateAngleMotorProfile(targetAngle), + () -> getSetTargetShooterAngleCommand(targetAngle), + (interrupted) -> {}, + () -> false, + SideShooter.getInstance() + ); + } +} 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..9889bccee --- /dev/null +++ b/src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooterConstants.java @@ -0,0 +1,107 @@ +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, ENCODER_ID = 0; + private static final double ANGLE_ENCODER_OFFSET = 0; + private static final CANSparkMax.IdleMode ANGLE_MOTOR_IDLE_MODE = CANSparkMax.IdleMode.kBrake; + private static final AbsoluteSensorRangeValue ANGLE_ENCODER_VALUE = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; + private static final SensorDirectionValue ANGLE_ENCODER_SENSOR_DIRECTION = SensorDirectionValue.CounterClockwise_Positive; + private static final InvertedValue SHOOTER_INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; + private static final boolean ANGLE_MOTOR_INVERTED = false; + private static final NeutralModeValue SHOOTING_NEUTRAL_MODE_VALUE = NeutralModeValue.Coast; + private static final double VOLTAGE_COMPENSATION_SATURATION = 12; + static final TalonFX SHOOTING_MOTOR = new TalonFX(SHOOTING_MOTOR_ID); + static final CANSparkMax ANGLE_MOTOR = new CANSparkMax(ANGLE_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless); + private static final CANcoder ANGLE_ENCODER = new CANcoder(ENCODER_ID); + + 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 = 0, + 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.58835, + ANGLE_MOTOR_KV = 0.74627, + ANGLE_MOTOR_KA = 0.37502, + ANGLE_MOTOR_KG = 0.92056; + static final ArmFeedforward SIDE_SHOOTER_FEEDFORWARD = new ArmFeedforward( + ANGLE_MOTOR_KS, ANGLE_MOTOR_KG, ANGLE_MOTOR_KV, ANGLE_MOTOR_KA + ); + + static final StatusSignal ANGLE_ENCODER_POSITION_SIGNAL = ANGLE_ENCODER.getPosition(), ANGLE_ENCODER_VELOCITY_SIGNAL = ANGLE_ENCODER.getVelocity(); + + static { + configureAngleEncoder(); + configureAngleMotor(); + configureShootingMotor(); + } + + private static void configureShootingMotor() { + TalonFXConfiguration config = new TalonFXConfiguration(); + config.Audio.BeepOnBoot = false; + config.Audio.BeepOnConfig = false; + config.MotorOutput.Inverted = SHOOTER_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.setInverted(ANGLE_MOTOR_INVERTED); + ANGLE_MOTOR.setIdleMode(ANGLE_MOTOR_IDLE_MODE); + ANGLE_MOTOR.enableVoltageCompensation(VOLTAGE_COMPENSATION_SATURATION); + } + + private static void configureAngleEncoder() { + CANcoderConfiguration configureAngleMotor = new CANcoderConfiguration(); + configureAngleMotor.MagnetSensor.AbsoluteSensorRange = ANGLE_ENCODER_VALUE; + configureAngleMotor.MagnetSensor.MagnetOffset = ANGLE_ENCODER_OFFSET; + configureAngleMotor.MagnetSensor.SensorDirection = ANGLE_ENCODER_SENSOR_DIRECTION; + ANGLE_ENCODER.getConfigurator().apply(configureAngleMotor); + + ANGLE_ENCODER_VELOCITY_SIGNAL.setUpdateFrequency(100); + ANGLE_ENCODER_POSITION_SIGNAL.setUpdateFrequency(100); + ANGLE_ENCODER.optimizeBusUtilization(); + } + + public enum SideShooterState { + COLLECT_POSITION(Rotation2d.fromDegrees(0), 0), + MID_LEVEL_POSITION(Rotation2d.fromDegrees(222), 9), + HIGH_LEVEL_POSITION(Rotation2d.fromDegrees(666.3), 78); + + final Rotation2d angle; + final double shootingVoltage; + + SideShooterState(Rotation2d angle, double shootingVoltage) { + this.angle = angle; + this.shootingVoltage = shootingVoltage; + } + } +} \ No newline at end of file