Wrist now fully works(excluding talon, haven't calibrated its PID or …

…tested it at all), along with MoveWristToAngle command

src/main/java/commands/MoveWristToAngle.java

@@ -18,7 +18,12 @@ public MoveWristToAngle(Rotation2d angle) {
 
 	@Override
 	public void execute() {
-		wrist.GoToPosition(angle);
+		wrist.goToPosition(angle);
+	}
+
+	@Override
+	public boolean isFinished() {
+		return wrist.isAtAngle(angle);
 	}
 
 }

src/main/java/subsystems/wrist/IWrist.java

@@ -0,0 +1,13 @@
+package subsystems.wrist;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public interface IWrist {
+
+	public void updateInputs(WristInputsAutoLogged inputs);
+
+	public void goToPosition(Rotation2d position);
+
+	public void setPower(double power);
+
+}

src/main/java/subsystems/wrist/Wrist.java

@@ -1,16 +1,21 @@
 package subsystems.wrist;
 
-import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.Logger;
 import utils.GBSubsystem;
 
 public class Wrist extends GBSubsystem {
 
 	private static Wrist instance;
-	private static TalonSRX motor;
+	private IWrist iWrist;
+	private WristInputsAutoLogged inputs;
+	private Rotation2d targetAngle;
 
 	private Wrist() {
-		this.motor = new TalonSRX(WristConstants.MOTOR_ID);
+		this.iWrist = WristFactory.create();
+		this.inputs = new WristInputsAutoLogged();
+		this.targetAngle = WristConstants.PresetPositions.STARTING.ANGLE;
+		this.iWrist.updateInputs(inputs);
 	}
 
 	public static Wrist getInstance() {
@@ -20,16 +25,24 @@ public static Wrist getInstance() {
 		return instance;
 	}
 
-	public void GoToPosition(Rotation2d position) {
-		motor.set(WristConstants.PID_CONTROL_MODE, position.getRotations() % 1 * WristConstants.FULL_CIRCLE_ENCODER_TICKS);
+	public void goToPosition(Rotation2d position) {
+		this.targetAngle = position;
+	}
+
+	public boolean isAtAngle(Rotation2d angle) {
+		return Math.abs(inputs.angle.getRadians() - angle.getRadians()) <= WristConstants.ANGLE_TOLERANCE.getRadians();
 	}
 
 	@Override
 	protected String getLogPath() {
-		return "";
+		return "WRIST/";
 	}
 
 	@Override
-	protected void subsystemPeriodic() {}
+	protected void subsystemPeriodic() {
+		iWrist.goToPosition(targetAngle);
+		iWrist.updateInputs(inputs);
+		Logger.processInputs("wrist", inputs);
+	}
 
 }

src/main/java/subsystems/wrist/WristConstants.java

@@ -1,13 +1,35 @@
 package subsystems.wrist;
 
-import com.ctre.phoenix.motorcontrol.TalonSRXControlMode;
+import edu.wpi.first.math.geometry.Rotation2d;
 
 public class WristConstants {
 
-	protected static final int MOTOR_ID = 11;
 
-	protected static final TalonSRXControlMode PID_CONTROL_MODE = TalonSRXControlMode.Position;
+	public enum PresetPositions {
 
-	protected static final int FULL_CIRCLE_ENCODER_TICKS = 168;
+		STARTING(Rotation2d.fromDegrees(90)),
+		SCORE(Rotation2d.fromDegrees(190)),
+		SCORE_TRAP(Rotation2d.fromDegrees(390)), // find
+		TRANSFER(Rotation2d.fromDegrees(185)),
+		SAFE(Rotation2d.fromDegrees(180)),
+		INTAKE(Rotation2d.fromDegrees(180));
+
+		public final Rotation2d ANGLE;
+
+		PresetPositions(Rotation2d angle) {
+			this.ANGLE = angle;
+		}
+
+	}
+
+	public static final double LENGTH_OF_ENDEFFECTOR = 0.1;
+
+	public static final double WRIST_MASS_KG = 0.5;
+
+	public static final Rotation2d BACKWARD_ANGLE_LIMIT = Rotation2d.fromDegrees(-50);
+
+	public static final Rotation2d FORWARD_ANGLE_LIMIT = Rotation2d.fromDegrees(200);
+
+	public static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromRotations(0.05);
 
 }

src/main/java/subsystems/wrist/WristFactory.java

@@ -0,0 +1,17 @@
+package subsystems.wrist;
+
+import subsystems.wrist.simulationWrist.SimulationWrist;
+import subsystems.wrist.talonWrist.TalonWrist;
+import training.Robot;
+
+public class WristFactory {
+
+	public static IWrist create() {
+		return switch (Robot.ROBOT_TYPE) {
+			case REAL -> new TalonWrist();
+			case SIMULATION -> new SimulationWrist();
+		};
+	}
+
+
+}

src/main/java/subsystems/wrist/WristInputs.java

@@ -0,0 +1,11 @@
+package subsystems.wrist;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.AutoLog;
+
+@AutoLog
+public class WristInputs {
+
+	public Rotation2d angle;
+
+}

src/main/java/subsystems/wrist/simulationWrist/SimulationWrist.java

@@ -0,0 +1,57 @@
+package subsystems.wrist.simulationWrist;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import subsystems.GlobalConstants;
+import subsystems.elbow.simulationElbow.SimulationElbowConstants;
+import subsystems.wrist.IWrist;
+import subsystems.wrist.WristConstants;
+import subsystems.wrist.WristInputsAutoLogged;
+
+public class SimulationWrist implements IWrist {
+
+	private final SingleJointedArmSim motor;
+
+	private final PIDController controller;
+
+	public SimulationWrist() {
+		motor = new SingleJointedArmSim(
+			DCMotor.getNEO(SimulationWristConstants.NUMBER_OF_MOTORS),
+			SimulationWristConstants.GEAR_RATIO,
+			SingleJointedArmSim.estimateMOI(WristConstants.LENGTH_OF_ENDEFFECTOR, WristConstants.WRIST_MASS_KG),
+			WristConstants.LENGTH_OF_ENDEFFECTOR,
+			WristConstants.BACKWARD_ANGLE_LIMIT.getRadians(),
+			WristConstants.FORWARD_ANGLE_LIMIT.getRadians(),
+			false,
+			WristConstants.PresetPositions.STARTING.ANGLE.getRadians()
+		);
+
+		controller = new PIDController(SimulationWristConstants.KP, SimulationWristConstants.KI, SimulationWristConstants.KD);
+	}
+
+
+	@Override
+	public void setPower(double power) {
+		setVoltage(power * GlobalConstants.SIMULATION_BATTERY_VOLTAGE);
+	}
+
+	private void setVoltage(double voltage) {
+		double limited_voltage = Math
+			.min((Math.max(voltage, -GlobalConstants.SIMULATION_BATTERY_VOLTAGE)), GlobalConstants.SIMULATION_BATTERY_VOLTAGE);
+		motor.setInputVoltage(limited_voltage);
+	}
+
+	@Override
+	public void updateInputs(WristInputsAutoLogged inputs) {
+		inputs.angle = Rotation2d.fromRadians(motor.getAngleRads());
+		motor.update(SimulationElbowConstants.MOTOR_UPDATE_PERIOD);
+	}
+
+	@Override
+	public void goToPosition(Rotation2d position) {
+		setVoltage(controller.calculate(motor.getAngleRads(), position.getRadians()));
+	}
+
+}

src/main/java/subsystems/wrist/simulationWrist/SimulationWristConstants.java

@@ -0,0 +1,15 @@
+package subsystems.wrist.simulationWrist;
+
+public class SimulationWristConstants {
+
+	public static final int NUMBER_OF_MOTORS = 1;
+
+	public static final double GEAR_RATIO = 1;
+
+	public static final double KP = 0.12;
+
+	public static final double KI = 0;
+
+	public static final double KD = 0.025;
+
+}

src/main/java/subsystems/wrist/talonWrist/TalonWrist.java

@@ -0,0 +1,35 @@
+package subsystems.wrist.talonWrist;
+
+import com.ctre.phoenix.motorcontrol.TalonSRXControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import edu.wpi.first.math.geometry.Rotation2d;
+import subsystems.wrist.IWrist;
+import subsystems.wrist.WristInputsAutoLogged;
+
+public class TalonWrist implements IWrist {
+
+	private static TalonSRX motor;
+
+	public TalonWrist() {
+		this.motor = new TalonSRX(TalonWristConstants.MOTOR_ID);
+	}
+
+
+	public void goToPosition(Rotation2d position) {
+		motor.set(
+			TalonWristConstants.PID_CONTROL_MODE,
+			position.getRotations() % 1 * TalonWristConstants.FULL_CIRCLE_ENCODER_TICKS
+		);
+	}
+
+	@Override
+	public void setPower(double power) {
+		motor.set(TalonSRXControlMode.PercentOutput, power);
+	}
+
+	@Override
+	public void updateInputs(WristInputsAutoLogged inputs) {
+		inputs.angle = Rotation2d.fromRotations(motor.getSelectedSensorPosition());
+	}
+
+}

src/main/java/subsystems/wrist/talonWrist/TalonWristConstants.java

@@ -0,0 +1,13 @@
+package subsystems.wrist.talonWrist;
+
+import com.ctre.phoenix.motorcontrol.TalonSRXControlMode;
+
+public class TalonWristConstants {
+
+	protected static final int MOTOR_ID = 11;
+
+	protected static final TalonSRXControlMode PID_CONTROL_MODE = TalonSRXControlMode.Position;
+
+	protected static final int FULL_CIRCLE_ENCODER_TICKS = 168;
+
+}