Shooter pull request

build.gradle

@@ -2,7 +2,7 @@ import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO
 
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2022.1.1"
+    id "edu.wpi.first.GradleRIO" version "2022.2.1"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

src/main/java/org/usfirst/frc4904/robot/RobotMap.java

@@ -2,7 +2,13 @@
 
 import edu.wpi.first.wpilibj2.command.Subsystem;
 import org.usfirst.frc4904.standard.custom.controllers.CustomJoystick;
+import org.usfirst.frc4904.standard.custom.motioncontrollers.CANTalonFX;
+import org.usfirst.frc4904.standard.custom.motioncontrollers.CANTalonSRX;
+import org.usfirst.frc4904.standard.custom.motioncontrollers.CustomPIDController;
 import org.usfirst.frc4904.standard.custom.controllers.CustomXbox;
+import org.usfirst.frc4904.standard.custom.sensors.CANTalonEncoder;
+import org.usfirst.frc4904.robot.subsystems.Flywheel;
+import org.usfirst.frc4904.standard.custom.CustomPIDSourceType;
 
 public class RobotMap {
     public static class Port {
@@ -12,6 +18,8 @@ public static class HumanInput {
         }
 
         public static class CANMotor {
+            public static final int FLYWHEEL_MOTOR_A = -1; // TODO: set port
+            public static final int FLYWHEEL_MOTOR_B = -1;
         }
 
         public static class PWM {
@@ -39,9 +47,28 @@ public static class Chassis {
             public static final double INCHES_PER_TICK = Metrics.Chassis.CIRCUMFERENCE_INCHES
                     / Metrics.Chassis.TICKS_PER_REVOLUTION;
         }
+
+        public static class Encoders {
+            public static class TalonEncoders {
+                public static final double TICKS_PER_REVOLUTION = 2048.0;
+                public static final double REVOLUTIONS_PER_TICK = 1 / TICKS_PER_REVOLUTION;
+            }
+
+            public static class CANCoders {
+                public static final double TICKS_PER_REVOLUTION = 4096.0;
+                public static final double REVOLUTIONS_PER_TICK = 1 / TICKS_PER_REVOLUTION;
+            }
+        }
     }
 
     public static class PID {
+        public static class Flywheel {
+            public static final double P = -1; // TODO: tune PID constants
+            public static final double I = -1;
+            public static final double D = -1;
+            public static final double F = -1;
+        }
+
         public static class Drive {
         }
 
@@ -51,6 +78,10 @@ public static class Turn {
     }
 
     public static class Component {
+        public static CANTalonEncoder flywheelEncoderA;
+        public static CANTalonEncoder flywheelEncoderB;
+        public static Flywheel shooter;
+        public static CustomPIDController flywheelPID;
     }
 
     public static class Input {
@@ -70,5 +101,21 @@ public RobotMap() {
         HumanInput.Driver.xbox = new CustomXbox(Port.HumanInput.xboxController);
 		HumanInput.Operator.joystick = new CustomJoystick(Port.HumanInput.joystick);
 
+        CANTalonFX flywheelATalon = new CANTalonFX(Port.CANMotor.FLYWHEEL_MOTOR_A);
+        flywheelATalon.setInverted(true); // todo: check if flywheel is in the correct direction (check if it is inverted)
+        CANTalonFX flywheelBTalon = new CANTalonFX(Port.CANMotor.FLYWHEEL_MOTOR_B);
+
+        Component.flywheelEncoderB = new CANTalonEncoder(flywheelBTalon, false,
+            Metrics.Encoders.TalonEncoders.REVOLUTIONS_PER_TICK);
+        Component.flywheelEncoderB.setCustomPIDSourceType(CustomPIDSourceType.kRate);
+        /** Motion Controllers */
+        Component.flywheelPID = new CustomPIDController(PID.Flywheel.P, PID.Flywheel.I, PID.Flywheel.D, PID.Flywheel.F,
+                Component.flywheelEncoderB);
+
+        Component.shooter = new Flywheel("Shooter", Component.flywheelPID, flywheelATalon, flywheelBTalon);
+        /** Classes */
+        // Component.intake = new Intake(Component.intakeRollerMotor,
+        // Component.liftBeltMotor, Component.funnelMotor,
+        // Component.intakeSolenoid);
     }
-}
+}

src/main/java/org/usfirst/frc4904/robot/commands/FlywheelOff.java

@@ -0,0 +1,20 @@
+package org.usfirst.frc4904.robot.commands;
+
+import org.usfirst.frc4904.robot.RobotMap;
+import org.usfirst.frc4904.robot.subsystems.Flywheel;
+
+public class FlywheelOff extends FlywheelSetSpeed {
+
+  /**
+   * Set flywheel speed to zero
+   * 
+   * @param flywheel The flywheel to manipulate
+   */
+  public FlywheelOff(Flywheel flywheel) {
+    super(flywheel, 0.0);
+  }
+
+  public FlywheelOff() {
+    super(RobotMap.Component.shooter, 0.0);
+  }
+}

src/main/java/org/usfirst/frc4904/robot/commands/FlywheelSetSpeed.java

@@ -0,0 +1,22 @@
+package org.usfirst.frc4904.robot.commands;
+
+import org.usfirst.frc4904.robot.RobotMap;
+import org.usfirst.frc4904.robot.subsystems.Flywheel;
+import org.usfirst.frc4904.standard.commands.motor.MotorVelocitySet;
+
+public class FlywheelSetSpeed extends MotorVelocitySet {
+
+  /**
+   * Spin up the flywheel to a speed
+   * 
+   * @param flywheel The flywheel to manipulate
+   * @param speed    The speed to spin the flywheel up to
+   */
+  public FlywheelSetSpeed(Flywheel flywheel, double speed) {
+    super("FlywheelMaintainSpeed", flywheel, speed);
+  }
+
+  public FlywheelSetSpeed(double speed) {
+    this(RobotMap.Component.shooter, speed);
+  }
+}

src/main/java/org/usfirst/frc4904/robot/subsystems/Flywheel.java

@@ -0,0 +1,115 @@
+package org.usfirst.frc4904.robot.subsystems;
+
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+
+import org.usfirst.frc4904.standard.custom.motioncontrollers.MotionController;
+import org.usfirst.frc4904.standard.subsystems.motor.VelocitySensorMotor;
+import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.IdentityModifier;
+import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.SpeedModifier;
+
+/**
+ * Flywheel Base Class
+ * 
+ * ! This class does not include a piston, use Shooter.java for the 2020-season
+ * ! specific flywheel subsystem.
+ * 
+ * This class is designed to go into standard, eventually.
+ */
+public class Flywheel extends VelocitySensorMotor {
+  public static enum FlywheelStatus {
+    IDLE, SPINNING_UP, AT_SPEED
+  }
+
+  protected FlywheelStatus currentStatus = FlywheelStatus.IDLE;
+  protected double targetSpeed = 0.0;
+
+  /**
+   * Flywheel constructor - extends VelocitySensorMotor
+   * motionController.absoluteTolerance should be set.
+   * 
+   * @param name             The name of the flywheel
+   * @param SpeedModifier    The speedModifier
+   * @param motionController The motionController
+   * @param motors           Motors to control
+   */
+  public Flywheel(String name, SpeedModifier speedModifier, MotionController motionController,
+    MotorController... motors) {
+    super(name, speedModifier, motionController, motors);
+    // super.enableMotionController();
+  }
+
+  /**
+   * Flywheel constructor - extends VelocitySensorMotor
+   * 
+   * @param SpeedModifier    The speedModifier
+   * @param motionController The motionController
+   * @param motors           Motors to control
+   */
+  public Flywheel(SpeedModifier speedModifier, MotionController motionController, MotorController... motors) {
+    this("Flywheel", speedModifier, motionController, motors);
+  }
+
+  /**
+   * Flywheel constructor - extends VelocitySensorMotor
+   * 
+   * @param name             The name of the flywheel
+   * @param motionController The motionController
+   * @param motors           Motors to control
+   */
+  public Flywheel(String name, MotionController motionController, MotorController... motors) {
+    this(name, new IdentityModifier(), motionController, motors);
+  }
+
+  /**
+   * Flywheel constructor - extends VelocitySensorMotor
+   * 
+   * @param motionController The motionController
+   * @param motors           Motors to control
+   */
+  public Flywheel(MotionController motionController, MotorController... motors) {
+    this("Flywheel", motionController, motors);
+  }
+
+  @Override
+  public void periodic() {
+    syncStatus();
+  }
+
+  protected void syncStatus() {
+    // TODO:// Infinitely idle?
+    if (Math.abs(targetSpeed) < motionController.getAbsoluteTolerance() || currentStatus == FlywheelStatus.IDLE) {
+      currentStatus = FlywheelStatus.IDLE;
+      return;
+    }
+    if (motionController.onTarget()) {
+      currentStatus = FlywheelStatus.AT_SPEED;
+    } else {
+      currentStatus = FlywheelStatus.SPINNING_UP;
+    }
+  }
+
+  public FlywheelStatus getStatus() {
+    return currentStatus;
+  }
+
+  public double getVelocity() {
+    return motionController.getSensorValue();
+  }
+
+  public double getTargetVelocity() {
+    return targetSpeed;
+  }
+
+  public void setVelocity() {
+    super.set(targetSpeed);
+  }
+
+  public void setVelocity(double speed) {
+    targetSpeed = speed;
+    setVelocity();
+  }
+
+  public MotionController getMC() {
+    return motionController;
+  }
+}