Merge pull request #30 from frc-862/27-shooter-subsystem

27 shooter subsystem

src/main/java/frc/robot/Constants.java

@@ -176,11 +176,7 @@ public class FlywheelConstants {
         public static final double FLYWHEEL_MOTOR_KS = 0;
         public static final double FLYWHEEL_MOTOR_KV = 0;
 
-        public static final double FLYWHEEL_RPM_TOLERANCE = 0;
-        // Distance in meters, angle in degrees
-        public static final InterpolationMap SHOOTER_DISTANCE_ANGLE_MAP = new InterpolationMap();
-        // Distance in meters, speed in RPM
-        public static final InterpolationMap SHOOTER_DISTANCE_SPEED_MAP = new InterpolationMap();
+        public static final double RPM_TOLERANCE = 0;
     }
 
     public class PivotConstants {
@@ -193,7 +189,31 @@ public class PivotConstants {
         public static final double PIVOT_MOTOR_KD = 0;        
         public static final double PIVOT_MOTOR_KS = 0;
         public static final double PIVOT_MOTOR_KV = 0;
-        public static final double PIVOT_TOLERANCE = 0;
 
+        public static final double ANGLE_TOLERANCE = 0;
+
+    }
+
+    public class ShooterConstants {
+        public static final double BASE_RPM = 0;
+        public static final double STOW_ANGLE = 0;
+
+
+        public enum SHOOTER_STATES {
+            STOW,
+            PRIME,
+            AIM,
+            SHOOT
+        }
+
+        // Distance in meters, angle in degrees
+        public static final InterpolationMap ANGLE_MAP = new InterpolationMap() {{
+            put(0d, 0d);
+        }};
+
+        // Distance in meters, speed in RPM
+        public static final InterpolationMap SPEED_MAP = new InterpolationMap() {{
+            put(0d, 0d);
+        }};
     }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -19,7 +19,6 @@
 import frc.robot.Constants.ControllerConstants;
 import frc.robot.Constants.TunerConstants;
 import frc.robot.Constants.drivetrainConstants;
-import frc.robot.command.Shoot;
 import frc.robot.subsystems.Swerve;
 import frc.thunder.LightningContainer;
 

src/main/java/frc/robot/subsystems/Flywheel.java

@@ -12,7 +12,9 @@ public class Flywheel extends SubsystemBase {
     private TalonFX shooterMotor1;
     private TalonFX shooterMotor2;
 
-    final VelocityVoltage rpmTarget = new VelocityVoltage(0).withSlot(0);
+    private double targetRPM = 0;
+
+    private final VelocityVoltage rpmPID = new VelocityVoltage(0).withSlot(0);
 
     public Flywheel() {
         shooterMotor1 = FalconConfig.createMotor(CAN.FLYWHEEL_MOTOR_1, getName(), FlywheelConstants.FLYWHEEL_MOTOR_1_INVERT, FlywheelConstants.FLYWHEEL_MOTOR_SUPPLY_CURRENT_LIMIT, FlywheelConstants.FLYWHEEL_MOTOR_STATOR_CURRENT_LIMIT, FlywheelConstants.FLYWHEEL_MOTOR_NEUTRAL_MODE, FlywheelConstants.FLYWHEEL_MOTOR_KP, FlywheelConstants.FLYWHEEL_MOTOR_KI, FlywheelConstants.FLYWHEEL_MOTOR_KD, FlywheelConstants.FLYWHEEL_MOTOR_KS, FlywheelConstants.FLYWHEEL_MOTOR_KV);
@@ -24,8 +26,10 @@ public Flywheel() {
      * @param rpm RPM of the flywheel
      */
     public void setRPM(double rpm){
-        shooterMotor1.setControl(rpmTarget.withVelocity(rpm));
-        shooterMotor2.setControl(rpmTarget.withVelocity(rpm));
+        targetRPM = rpm;
+
+        shooterMotor1.setControl(rpmPID.withVelocity(rpm));
+        shooterMotor2.setControl(rpmPID.withVelocity(rpm));
     }
 
     /**
@@ -34,4 +38,11 @@ public void setRPM(double rpm){
     public double getFlywheelRPM() {
         return (shooterMotor1.getVelocity().getValue() + shooterMotor2.getVelocity().getValue()) / 2;
     }
+
+    /**
+     * @return Whether or not the flywheel is on target, within FlywheelConstants.RPM_TOLERANCE
+     */
+    public boolean onTarget() {
+        return Math.abs(getFlywheelRPM() - targetRPM) < FlywheelConstants.RPM_TOLERANCE;
+    }
 }

src/main/java/frc/robot/subsystems/Pivot.java

@@ -10,8 +10,10 @@
 
 public class Pivot extends SubsystemBase {
     private TalonFX angleMotor;
-
-    final PositionVoltage angleTarget = new PositionVoltage(0).withSlot(0);
+    private final PositionVoltage anglePID = new PositionVoltage(0).withSlot(0);
+
+    //TODO: find initial target angle
+    private double targetAngle = 0;
 
     public Pivot() {
         angleMotor = FalconConfig.createMotor(CAN.FLYWHEEL_MOTOR_1, getName(), PivotConstants.PIVOT_MOTOR_INVERT, PivotConstants.PIVOT_MOTOR_SUPPLY_CURRENT_LIMIT, PivotConstants.PIVOT_MOTOR_STATOR_CURRENT_LIMIT, PivotConstants.PIVOT_MOTOR_NEUTRAL_MODE, PivotConstants.PIVOT_MOTOR_KP, PivotConstants.PIVOT_MOTOR_KI, PivotConstants.PIVOT_MOTOR_KD, PivotConstants.PIVOT_MOTOR_KS, PivotConstants.PIVOT_MOTOR_KV);
@@ -22,7 +24,8 @@ public Pivot() {
      * @param angle Angle of the pivot
      */
     public void setAngle(double angle) {
-        angleMotor.setControl(angleTarget.withPosition(angle));
+        targetAngle = angle;
+        angleMotor.setControl(anglePID.withPosition(angle));
     }
 
     /**
@@ -31,4 +34,11 @@ public void setAngle(double angle) {
     public double getAngle() {
         return angleMotor.getPosition().getValue();
     }
+
+    /**
+     * @return Whether or not the pivot is on target, within PivotConstants.ANGLE_TOLERANCE
+     */
+    public boolean onTarget() {
+        return Math.abs(getAngle() - targetAngle) < PivotConstants.ANGLE_TOLERANCE;
+    }
 }

src/main/java/frc/robot/subsystems/Shooter.java

@@ -0,0 +1,68 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.ShooterConstants;
+import frc.robot.Constants.ShooterConstants.SHOOTER_STATES;
+
+public class Shooter extends SubsystemBase {
+    private Pivot pivot;
+    private Flywheel flywheel;
+    //TODO: add collector/indexer
+
+    //TODO: find initial target angle
+    private double currentDistance = 0;
+
+    private SHOOTER_STATES state = SHOOTER_STATES.STOW;
+
+    public Shooter(Pivot pivot, Flywheel flywheel) {
+        this.pivot = pivot;
+        this.flywheel = flywheel;
+    }
+
+    @Override
+    public void periodic() {
+        //TODO: IMPLEMENT VISION/ODO
+        currentDistance = 0;
+        switch (state) {
+            //Stow pivot to allow collector to input not\
+            case STOW:
+                pivot.setAngle(ShooterConstants.STOW_ANGLE);
+                flywheel.setRPM(0);
+                break;
+            //Prime is warming up the flywheel for shooting
+            case PRIME:
+                pivot.setAngle(ShooterConstants.STOW_ANGLE);
+                flywheel.setRPM(ShooterConstants.BASE_RPM);
+                break;
+            //Aim is targeting the pivot toward the target and speeding up the flywheel
+            case AIM:
+                pivot.setAngle(ShooterConstants.ANGLE_MAP.get(currentDistance));
+                flywheel.setRPM(ShooterConstants.SPEED_MAP.get(currentDistance));
+                break;
+            // Shoot is shooting the note
+            case SHOOT:
+                pivot.setAngle(ShooterConstants.ANGLE_MAP.get(currentDistance));         
+                flywheel.setRPM(ShooterConstants.SPEED_MAP.get(currentDistance));
+
+                //If flywheel and pivot are on target (ready to shoot), shoot               
+                if(flywheel.onTarget() && pivot.onTarget()) {
+                    //index
+                }
+                break;
+        }
+    }
+
+    public SHOOTER_STATES getState() {
+        return state;
+
+}
+
+    /**
+     * Sets the state of the shooter (STOW, PRIME, AIM, SHOOT)
+     * note: 
+     * @param state
+     */
+    public void setState(SHOOTER_STATES state) {
+        this.state = state;
+    }
+}