Turret

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

@@ -7,6 +7,7 @@
 
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
 import com.stuypulse.robot.constants.Ports;
+import com.stuypulse.robot.subsystems.turret.Turret;
 import com.stuypulse.stuylib.input.Gamepad;
 import com.stuypulse.stuylib.input.gamepads.AutoGamepad;
 
@@ -21,6 +22,7 @@ public class RobotContainer {
     public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR);
 
     // Subsystem
+    public final Turret turret = Turret.getInstance();
 
     // Autons
     private static SendableChooser<Command> autonChooser = new SendableChooser<>();

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

@@ -14,4 +14,11 @@
  * We use StuyLib's SmartNumber / SmartBoolean in order to have tunable
  * values that we can edit on Shuffleboard.
  */
-public interface Settings {}
+public interface Settings {
+    double DT = 0.02;
+    public interface Turret {
+        int port = 0;
+        SmartNumber kV = new SmartNumber("Turret kV", 3);
+        SmartNumber kA = new SmartNumber("Turret kA", 0.5);
+    }
+}

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

@@ -0,0 +1,65 @@
+package com.stuypulse.robot.subsystems.turret;
+
+import com.stuypulse.robot.Robot;
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.stuylib.control.Controller;
+import com.stuypulse.stuylib.control.feedback.PIDController;
+import com.stuypulse.stuylib.network.SmartNumber;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public abstract class Turret extends SubsystemBase {
+
+    public static final Turret instance;
+
+    static {
+        if(Robot.isReal()) {
+            instance = new TurretImpl(Settings.Turret.port);
+        }
+        else {
+            instance = new TurretSim();
+        }
+    }
+
+    public static Turret getInstance() {
+        return instance;
+    }
+
+    public Controller controller;
+    public SmartNumber targetAngle = new SmartNumber("Target Angle", 0);
+
+    public void stop() {
+        setTurretVoltage(0);
+    }
+
+    public Turret() {
+        controller = new PIDController(3, 0, 0); 
+    }
+
+    public abstract double getTurretAngle();
+    public abstract void setTurretVoltage(double voltage);
+
+    public void setTurretAngle(double angle) {
+        targetAngle.set(angle);
+    }
+
+    @Override
+    public final void periodic() {
+        controller.update(
+            targetAngle.get(), 
+            getTurretAngle()
+        );
+
+        double output = controller.getOutput();
+        setTurretVoltage(output);
+        SmartDashboard.putNumber("Calculated Voltage", output);
+        SmartDashboard.putNumber("Turret Angle", getTurretAngle());
+
+        periodic2();
+    }
+
+    public abstract void periodic2();
+
+}
+

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

@@ -0,0 +1,32 @@
+package com.stuypulse.robot.subsystems.turret;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+import com.revrobotics.RelativeEncoder;
+
+public class TurretImpl extends Turret {
+
+    private CANSparkMax motor;
+    private RelativeEncoder encoder;
+
+    public TurretImpl(int port) {
+        motor = new CANSparkMax(port, MotorType.kBrushless);
+        encoder = motor.getEncoder();
+    }
+
+    @Override
+    public double getTurretAngle() {
+        return encoder.getPosition();
+    }
+
+    @Override
+    public void setTurretVoltage(double voltage) {
+        motor.setVoltage(voltage);
+    }
+
+    @Override
+    public void periodic2() {
+
+    }
+}
+

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

@@ -0,0 +1,54 @@
+package com.stuypulse.robot.subsystems.turret;
+
+
+import com.stuypulse.robot.constants.Settings;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.simulation.LinearSystemSim;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class TurretSim extends Turret {
+
+    private LinearSystemSim<N2, N1, N1> turretSim;
+    private final FieldObject2d turretPose2d;
+
+    private Field2d field;
+
+    public TurretSim() {
+        turretSim = new LinearSystemSim<N2, N1, N1>(
+            LinearSystemId.identifyPositionSystem(Settings.Turret.kV.get(), Settings.Turret.kA.get())
+        );
+
+        field = new Field2d();
+        turretPose2d = field.getObject("Turret Pose 2d");
+
+        turretPose2d.setPose(new Pose2d(4, 4, Rotation2d.fromDegrees(0)));
+        SmartDashboard.putData(field);
+    }
+
+    @Override
+    public double getTurretAngle() {
+        return turretSim.getOutput(0);
+    }
+
+    @Override
+    public void setTurretVoltage(double voltage) {
+        turretSim.setInput(MathUtil.clamp(voltage, -12, 12));
+    }
+
+    @Override
+    public void periodic2() {
+        turretSim.update(Settings.DT);
+        turretPose2d.setPose(new Pose2d(
+            turretPose2d.getPose().getTranslation(),
+            Rotation2d.fromDegrees(getTurretAngle())
+        ));
+    }
+}