Azoulay

src/main/java/frc/trigon/robot/subsystems/sideshooter/SideShooter.java

@@ -0,0 +1,74 @@
+package frc.trigon.robot.subsystems.sideshooter;
+
+
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.math.controller.ArmFeedforward;
+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.Command;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.trigon.robot.utilities.Conversions;
+
+import javax.swing.text.Position;
+import java.lang.annotation.Target;
+
+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 lastAngleMotorProfileGeneration;
+
+    public static SideShooter getInstance() {
+        return INSTANCE;
+    }
+    private final static SideShooter INSTANCE = new SideShooter();
+
+    Rotation2d getAngleMotorPosition() {
+        double positionRevolutions = SideShooterConstants.ANGLE_ENCODER_POSITION_SIGNAL.refresh().getValue();
+        return Rotation2d.fromRotations(positionRevolutions);
+    }
+
+    Double getAngleMotorVelocityRotationsPerSecond() {
+        double perHundredMsToPerSecond = Conversions.perHundredMsToPerSecond(SideShooterConstants.ANGLE_ENCODER_VELOCITY_SIGNAL.refresh().getValue());
+        return Conversions.revolutionsToDegrees(perHundredMsToPerSecond);
+    }
+
+    double getAngleMotorProfileTime() {
+        return Timer.getFPGATimestamp() - lastAngleMotorProfileGeneration;
+    }
+
+    void generateAngleMotorProfile(Rotation2d targetAngle) {
+        angleMotorProfile = new TrapezoidProfile(SideShooterConstants.ANGLE_Constraints,
+                new TrapezoidProfile.State(targetAngle.getDegrees(), 0),
+                new TrapezoidProfile.State(getAngleMotorPosition().getDegrees(), getAngleMotorVelocityRotationsPerSecond()));
+
+        lastAngleMotorProfileGeneration = Timer.getFPGATimestamp();
+    }
+
+    double calculateAngleOutput(){
+    TrapezoidProfile.State targetState = angleMotorProfile.calculate(getAngleMotorProfileTime());
+    double pidOutPut = SideShooterConstants.ANGLE_PID_CONTROLLER.calculate(
+            getAngleMotorPosition().getDegrees(),
+            targetState.position
+    );
+    double Feedforward = SideShooterConstants.SIDE_SHOOTER_FEEDFORWARD.calculate(
+            Units.degreesToRadians(targetState.position),
+            Units.degreesToRadians(targetState.velocity),
+            targetState.position
+    );
+        angleMotor.setVoltage(pidOutPut = Feedforward);
+        return Feedforward+pidOutPut;
+    }
+
+    void setTargetAngleMotorProfileTime() {
+        if (angleMotorProfile == null) {
+            angleMotor.stopMotor();
+            return;
+        }
+        angleMotor.setVoltage(calculateAngleOutput());
+    }
+}

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()
+        );
+    }
+}

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<Double> 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;
+        }
+    }
+}