Made Turret Subsystem

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

@@ -3,6 +3,12 @@
 import edu.wpi.first.wpilibj2.command.Subsystem;
 import org.usfirst.frc4904.standard.custom.controllers.CustomJoystick;
 import org.usfirst.frc4904.standard.custom.controllers.CustomXbox;
+import org.usfirst.frc4904.standard.custom.motioncontrollers.CANTalonSRX;
+import org.usfirst.frc4904.standard.custom.motioncontrollers.CustomPIDController;
+import org.usfirst.frc4904.standard.custom.sensors.CANTalonEncoder;
+import org.usfirst.frc4904.standard.subsystems.motor.PositionSensorMotor;
+import org.usfirst.frc4904.robot.subsystems.Turret;
+
 
 public class RobotMap {
     public static class Port {
@@ -12,6 +18,7 @@ public static class HumanInput {
         }
 
         public static class CANMotor {
+            public static final int turretMotor = -1; // TODO: confirm port
         }
 
         public static class PWM {
@@ -48,9 +55,23 @@ public static class Drive {
         public static class Turn {
         }
 
+        public static class Turret {
+			public static final double P = -1; // TODO: TUNE
+			public static final double I = -1;
+			public static final double D = -1;
+			public static final double F = -1;
+			public static final double tolerance = -1;
+			public static final double dTolerance = -1;
+		}
+
     }
 
     public static class Component {
+        public static Turret turret;
+        public static CustomPIDController turretPID;
+        public static CANTalonEncoder turretEncoder;
+        public static CANTalonSRX turretMotor; // TODO: confirm motor type, could be srx
+
     }
 
     public static class Input {
@@ -69,6 +90,13 @@ public static class Operator {
     public RobotMap() {
         HumanInput.Driver.xbox = new CustomXbox(Port.HumanInput.xboxController);
 		HumanInput.Operator.joystick = new CustomJoystick(Port.HumanInput.joystick);
-
+
+        Component.turretMotor = new CANTalonSRX(Port.CANMotor.turretMotor);
+        Component.turretEncoder = new CANTalonEncoder(Component.turretMotor,
+			Turret.TICK_MULTIPLIER);
+        Component.turretPID = new CustomPIDController(PID.Turret.P,
+			PID.Turret.I, PID.Turret.D, PID.Turret.F,
+			Component.turretEncoder);
+        Component.turret = new Turret(new PositionSensorMotor("Turret", Component.turretPID, Component.turretMotor));
     }
 }

src/main/java/org/usfirst/frc4904/robot/commands/turret/TurnTurret.java

@@ -0,0 +1,56 @@
+package org.usfirst.frc4904.robot.commands.turret;
+
+import org.usfirst.frc4904.robot.RobotMap;
+import org.usfirst.frc4904.robot.subsystems.Turret;
+import org.usfirst.frc4904.standard.custom.sensors.InvalidSensorException;
+import org.usfirst.frc4904.standard.subsystems.motor.PositionSensorMotor;
+import org.usfirst.frc4904.standard.LogKitten;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class TurnTurret extends CommandBase {
+	protected PositionSensorMotor turretMotor;
+	protected double turretPosition;
+
+	public TurnTurret(String name, double turretPosition) {
+		super();
+		this.setName(name);
+		this.turretMotor = RobotMap.Component.turret.turretMotor;
+		this.turretPosition = turretPosition;
+		addRequirements(RobotMap.Component.turret);
+	}
+
+	public TurnTurret(double turretPosition) {
+		super();
+		this.setName("TurnTurret");
+		this.turretMotor = RobotMap.Component.turret.turretMotor;
+		this.turretPosition = turretPosition;
+		addRequirements(RobotMap.Component.turret);
+	}
+
+	@Override
+	public void initialize() {
+		try {
+			this.turretMotor.reset();
+			this.turretMotor.enableMotionController();
+			this.turretMotor.setPositionSafely(turretPosition * Turret.GEAR_RATIO); 
+		} catch (InvalidSensorException e) {
+			LogKitten.e("InvalidSensorException in TurnTurret.initialize()");
+			cancel();
+		}
+	}
+
+	@Override
+	public void execute() {
+		Exception potentialSensorException = this.turretMotor.checkSensorException();
+		if (potentialSensorException != null) {
+			LogKitten.e("SensorException in TurnTurret.execute()");
+			cancel();
+		}
+	}
+
+	@Override
+	public boolean isFinished() {
+		return false;
+	}
+}

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

@@ -0,0 +1,23 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.usfirst.frc4904.robot.subsystems;
+
+import org.usfirst.frc4904.standard.subsystems.motor.PositionSensorMotor;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Turret extends SubsystemBase {
+    public static final double ENCODER_TICKS = 1024; // TODO: CHANGE CONST, took this from the 2019 elevator
+	public static final double TICK_MULTIPLIER = 360.0 / ENCODER_TICKS; 
+    public static final double BIG_GEAR_RADIUS = -1; // Gear of the turret, TODO: CHANGE CONST
+    public static final double SMALL_GEAR_RADIUS = -1; // Gear of the motor, TODO: CHANGE CONST
+    public static final double GEAR_RATIO = BIG_GEAR_RADIUS / SMALL_GEAR_RADIUS;
+    public final PositionSensorMotor turretMotor; // TODO: confirm type of motor
+
+    /** Creates a new Turret. */
+    public Turret(PositionSensorMotor turretMotor) {
+        this.turretMotor = turretMotor;
+    }
+}