shooter and all that

simgui.json

@@ -2,7 +2,9 @@
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
+      "/LiveWindow/SimModule": "Subsystem",
       "/LiveWindow/Ungrouped/Scheduler": "Scheduler",
+      "/LiveWindow/Ungrouped/navX-Sensor[4]": "Gyro",
       "/SmartDashboard/Autonomous": "String Chooser",
       "/SmartDashboard/Field": "Field2d"
     },
@@ -26,7 +28,7 @@
   "NetworkTables": {
     "transitory": {
       "SmartDashboard": {
-        "Odometry": {
+        "Shooter": {
           "open": true
         },
         "open": true

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -8,8 +8,11 @@
 import com.stuypulse.robot.commands.TurretPoint;
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
 import com.stuypulse.robot.commands.odometry.OdometryRealign;
+import com.stuypulse.robot.commands.swerve.SwerveDriveDrive;
 import com.stuypulse.robot.constants.Ports;
 import com.stuypulse.robot.constants.Motors.Swerve;
+import com.stuypulse.robot.subsystems.shooter.Shooter;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
 import com.stuypulse.robot.subsystems.swerve.evenMoreSwerve.*;
 import com.stuypulse.robot.subsystems.turret.Turret;
 import com.stuypulse.stuylib.input.Gamepad;
@@ -35,6 +38,8 @@ public class RobotContainer {
 
     // Subsystem
     public final Turret turret = Turret.getInstance();
+    public final SwerveDrive swerve = SwerveDrive.getInstance();
+    public final Shooter shooter = Shooter.getInstance();
 
     // Autons
     private static SendableChooser<Command> autonChooser = new SendableChooser<>();
@@ -53,6 +58,7 @@ public RobotContainer() {
 
     private void configureDefaultCommands() {
         turret.setDefaultCommand(new TurretPoint (new Translation2d()));
+        swerve.setDefaultCommand(new SwerveDriveDrive(driver));
     }
 
     /***************/

src/main/java/com/stuypulse/robot/commands/TurretPoint.java

@@ -1,6 +1,5 @@
 package com.stuypulse.robot.commands;
 
-import com.stuypulse.robot.Robot;
 import com.stuypulse.robot.subsystems.odometry.Odometry;
 import com.stuypulse.robot.subsystems.turret.Turret;
 
@@ -32,7 +31,8 @@ public final void execute() {
             turret.setTargetAngle(0, 360, -360);
         }
 
-        else {turret.setTargetAngle(
+        else {
+            turret.setTargetAngle(
             (180 + Math.toDegrees(
                 Math.atan((robotPose.getY() - target.getY()) / (robotPose.getX() - target.getX()))
             )), 180, -180

src/main/java/com/stuypulse/robot/constants/Ports.java

@@ -7,12 +7,17 @@
 
 /** This file contains the different ports of motors, solenoids and sensors */
 public interface Ports {
+
     public interface Gamepad {
         int DRIVER = 0;
         int OPERATOR = 1;
         int DEBUGGER = 2;
     }
 
+    public interface Turret {
+        int TURN = -1;
+    }
+
     public interface Swerve {
         public interface FrontRight {
             int DRIVE = 10;
@@ -34,4 +39,5 @@ public interface BackRight {
             int TURN = 17;
         }
     }
+
 }

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -6,10 +6,13 @@
 package com.stuypulse.robot.constants;
 
 import com.pathplanner.lib.auto.PIDConstants;
+import com.stuypulse.stuylib.control.feedback.PIDController;
+// import com.stuypulse.stuylib.control.Controller;
 import com.stuypulse.stuylib.math.Vector2D;
 import com.stuypulse.stuylib.network.SmartBoolean;
 import com.stuypulse.stuylib.network.SmartNumber;
 
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
@@ -23,10 +26,31 @@
 public interface Settings {
 
     double DT = 0.02;
+
     public interface Turret {
-        int port = 0;
-        SmartNumber kV = new SmartNumber("Turret kV", 0.1);
-        SmartNumber kA = new SmartNumber("Turret kA", 0.01);
+        public interface Feedforward {
+            SmartNumber kV = new SmartNumber("Turret kV", 0.1);
+            SmartNumber kA = new SmartNumber("Turret kA", 0.01);
+        }
+    }
+
+    public interface Shooter {    
+        static double MAX_RPM = 5700.0;
+        static double MIN_RPM = 100.0;
+        static double MAX_RPM_CHANGE = 2000.0;
+        static double MAX_RPM_ERROR = 100.0;
+
+        public interface Feedback {
+            double kP = 0.005;
+            double kI = 0.0;
+            double kD = 0.00033;
+        }
+
+        public interface Feedforward {
+            double kS = 0.17118;
+            double kV = 0.0020763;
+            double kA = 0.00011861;
+        }
     }
 
     public interface Swerve {
@@ -154,4 +178,5 @@ public interface GyroFeedback {
     public static Vector2D vpow(Vector2D vec, double power) {
         return vec.mul(Math.pow(vec.magnitude(), power - 1));
     }
+
 }

src/main/java/com/stuypulse/robot/subsystems/shooter/Shooter.java

@@ -0,0 +1,82 @@
+package com.stuypulse.robot.subsystems.shooter;
+
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.stuylib.control.Controller;
+import com.stuypulse.stuylib.control.feedback.PIDController;
+import com.stuypulse.stuylib.control.feedforward.MotorFeedforward;
+
+// import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/**
+ * Shooter class contains the hardware logic for the Shooter class.
+ *
+ * <p>Contains a simple feedforward model of the shooter based on the voltage-balance equation and a
+ * PID controller to correct for any error.
+ *
+ * @author Richie Xue 
+ */
+public abstract class Shooter extends SubsystemBase{
+
+    private static final Shooter instance;
+
+    static {
+        if (RobotBase.isReal()) {
+            instance = new ShooterImpl();
+        }
+        else {
+            instance = new ShooterSim();
+        }
+    }
+
+    public static Shooter getInstance() {
+        return instance;
+    }
+
+    private final Controller controller;
+
+    private double targetRPM;
+
+    public Shooter() {
+        this.targetRPM = 0.0;
+
+        controller = new PIDController(Settings.Shooter.Feedback.kP, Settings.Shooter.Feedback.kI, Settings.Shooter.Feedback.kD)
+            .add(new MotorFeedforward(Settings.Shooter.Feedforward.kS, Settings.Shooter.Feedforward.kV, Settings.Shooter.Feedforward.kA).velocity());
+    }
+
+    public void setTargetRPM(double targetRPM) {
+        if (targetRPM < Settings.Shooter.MIN_RPM) {
+            this.targetRPM = 0.0;
+        } 
+        else if (targetRPM > Settings.Shooter.MAX_RPM) {
+            this.targetRPM = Settings.Shooter.MAX_RPM;
+        }
+        else {
+            this.targetRPM = targetRPM;
+        }
+    }
+
+    public double getTargetRPM() {
+        return this.targetRPM;
+    }
+
+    public abstract double getVelocity();
+    public abstract void setVoltage(double voltage);
+
+    @Override
+    public void periodic() {
+        if (getTargetRPM() < Settings.Shooter.MIN_RPM) {
+            this.setVoltage(0.0);
+        } 
+        else {
+            controller.update(targetRPM, getVelocity());
+            this.setVoltage(controller.getOutput());
+        }
+        SmartDashboard.putNumber("Shooter/Target RPM", getTargetRPM());
+        periodicallyCalled();
+    }
+
+    public abstract void periodicallyCalled();
+}

src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java

@@ -0,0 +1,41 @@
+package com.stuypulse.robot.subsystems.shooter;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+
+/**
+ * ShooterImpl class contains the hardware logic for the Shooter class.
+ *
+ * <p>Contains a simple feedforward model of the shooter based on the voltage-balance equation and a
+ * PID controller to correct for any error.
+ *
+ * @author Richie Xue 
+ */
+public class ShooterImpl extends Shooter {
+    private final CANSparkMax motor;
+    private final RelativeEncoder encoder;
+
+    public ShooterImpl() {
+        super();
+
+        this.motor = new CANSparkMax(1, CANSparkMax.MotorType.kBrushless);
+        this.encoder = motor.getEncoder();
+    }
+
+    public double getVelocity() {
+        return encoder.getVelocity();
+    }
+
+    @Override
+    public void setVoltage(double voltage) {
+        motor.setVoltage(voltage);
+    }   
+
+    @Override
+    public void periodicallyCalled() {
+        SmartDashboard.putNumber("Shooter/Velocity", getVelocity());
+        SmartDashboard.putNumber("Shooter/Error", getTargetRPM() - getVelocity());
+    }   
+}

src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterSim.java

@@ -0,0 +1,40 @@
+package com.stuypulse.robot.subsystems.shooter;
+
+import com.stuypulse.robot.constants.Settings;
+
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.simulation.LinearSystemSim;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class ShooterSim extends Shooter {
+
+    private LinearSystemSim<N1, N1, N1> shooterSim;
+
+    public ShooterSim() {
+        shooterSim = new LinearSystemSim<N1, N1, N1>(
+            LinearSystemId.identifyVelocitySystem(Settings.Shooter.Feedforward.kV, Settings.Shooter.Feedforward.kA)
+        );
+    }
+
+    @Override
+    public double getVelocity() {
+        return Units.radiansPerSecondToRotationsPerMinute(shooterSim.getOutput(0));
+    }
+
+    @Override
+    public void setVoltage(double voltage) {
+        shooterSim.setInput(voltage);
+    }
+
+    @Override
+    public void simulationPeriodic() {
+        shooterSim.update(Settings.DT);
+    }
+
+    @Override
+    public void periodicallyCalled() {
+        SmartDashboard.putNumber("Shooter/RPM", getVelocity());
+    }
+}

src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java

@@ -124,8 +124,6 @@ public ChassisSpeeds getChassisSpeeds() {
         return getKinematics().toChassisSpeeds(getModuleStates());
     }
 
-
-
     /** MODULE STATES API **/
     public void drive(Vector2D velocity, double omega) {
         ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(

src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java

@@ -1,6 +1,7 @@
 package com.stuypulse.robot.subsystems.turret;
 
 import com.stuypulse.robot.Robot;
+import com.stuypulse.robot.constants.Ports;
 import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.stuylib.control.Controller;
 import com.stuypulse.stuylib.control.feedback.PIDController;
@@ -15,7 +16,7 @@ public abstract class Turret extends SubsystemBase {
 
     static {
         if(Robot.isReal()) {
-            instance = new TurretImpl(Settings.Turret.port);
+            instance = new TurretImpl(Ports.Turret.TURN);
         }
         else {
             instance = new TurretSim();
@@ -65,8 +66,7 @@ public final void periodic() {
 
         double output = controller.getOutput();
         setTurretVoltage(output);
-
-        //SmartDashboard.putNumber("Calculated Voltage", output);
+
         SmartDashboard.putNumber("Calculated Voltage", 0);
         SmartDashboard.putNumber("Turret Angle", 0);
         SmartDashboard.putNumber("Target Angle", targetAngle.get());

src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java

@@ -2,16 +2,17 @@
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
-import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkMaxAbsoluteEncoder.Type;
+import com.revrobotics.SparkMaxAbsoluteEncoder;
 
 public class TurretImpl extends Turret {
 
     private CANSparkMax motor;
-    private RelativeEncoder encoder;
+    private SparkMaxAbsoluteEncoder encoder;
 
     public TurretImpl(int port) {
         motor = new CANSparkMax(port, MotorType.kBrushless);
-        encoder = motor.getEncoder();
+        encoder = motor.getAbsoluteEncoder(Type.kDutyCycle);
     }
 
     @Override

src/main/java/com/stuypulse/robot/subsystems/turret/TurretSim.java

@@ -20,12 +20,18 @@ public class TurretSim extends Turret {
 
     public TurretSim() {
         turretSim = new LinearSystemSim<N2, N1, N1>(
-            LinearSystemId.identifyPositionSystem(Settings.Turret.kV.get(), Settings.Turret.kA.get())
+            LinearSystemId.identifyPositionSystem(Settings.Turret.Feedforward.kV.get(), Settings.Turret.Feedforward.kA.get())
         );
 
         turretPose2d = Odometry.getInstance().getField().getObject("Turret Pose 2d");
 
-        turretPose2d.setPose(new Pose2d(4, 4, Rotation2d.fromDegrees(0)));
+        // turretPose2d.setPose(new Pose2d(4, 4, Rotation2d.fromDegrees(0)));
+        // turretPose2d.setPose(Odometry.getInstance().getPose());
+        turretPose2d.setPose(new Pose2d(
+            Odometry.getInstance().getTranslation(),
+            Rotation2d.fromDegrees(getTurretAngle())
+        ));
+
         SmartDashboard.putData(Odometry.getInstance().getField());
     }