Maya arm

src/main/java/training/commands/elbow/MoveElbowToAngle.java

@@ -8,21 +8,25 @@ public class MoveElbowToAngle extends Command {
 
 	private Rotation2d targetAngle;
 
+	public Elbow getInstance() {
+		return Elbow.getInstance();
+	}
+
 	public MoveElbowToAngle(Rotation2d targetAngle) {
 		this.targetAngle = targetAngle;
 	}
 
 	@Override
 	public void initialize() {
-		Elbow.getInstance().goToAngle(targetAngle);
+		getInstance().goToAngle(targetAngle);
 	}
 
 	public boolean isFinished() {
-		return Elbow.getInstance().isAtAngle(targetAngle);
+		return getInstance().isAtAngle(targetAngle);
 	}
 
 	public void end(boolean interrupted) {
-		Elbow.getInstance().stop();
+		getInstance().stayAtPosition();
 	}
 
-}
+}

src/main/java/training/commands/roller/MoveToSpeedRoller.java

@@ -7,17 +7,20 @@ public class MoveToSpeedRoller extends Command {
 
 	private double targetSpeed;
 
+	public Roller getInstance() {
+		return Roller.getInstance();
+	}
+
 	public void MoveToSpeedSpeed(double targetSpeed) {
 		this.targetSpeed = targetSpeed;
 	}
 
-
 	public boolean isFinished() {
-		return Roller.getInstance().isAtVelocity(targetSpeed);
+		return getInstance().isAtVelocity(targetSpeed);
 	}
 
 	public void end(boolean interrupted) {
-		Roller.getInstance().stop();
+		getInstance().stop();
 	}
 
-}
+}

src/main/java/training/commands/wrist/MoveToAngleWrist.java → src/main/java/training/commands/wrist/MoveWristToAngle.java

@@ -4,26 +4,29 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import training.subsystems.wrist.Wrist;
 
-public class MoveToAngleWrist extends Command {
+public class MoveWristToAngle extends Command {
 
 	private Rotation2d targetAngle;
 
-	public MoveToAngleWrist(Rotation2d targetAngle) {
+	public Wrist getInstance() {
+		return Wrist.getInstance();
+	}
+
+	public MoveWristToAngle(Rotation2d targetAngle) {
 		this.targetAngle = targetAngle;
 	}
 
 	@Override
 	public void initialize() {
-		Wrist.goToAngle(targetAngle);
+		getInstance().goToAngle(targetAngle);
 	}
 
 	public boolean isFinished() {
-		return Wrist.getInstance().isAtAngle(targetAngle);
+		return getInstance().isAtAngle(targetAngle);
 	}
 
 	public void end(boolean interrupted) {
-		Wrist.getInstance().stop();
+		getInstance().stop();
 	}
 
-}
-
+}

src/main/java/training/subsystems/elbow/Elbow.java

@@ -9,16 +9,16 @@
 
 public class Elbow extends GBSubsystem {
 
-	private final CANSparkMax motor;
 	private static Elbow instance;
-	private ArmFeedforward armFeedforward;
+	private final CANSparkMax motor;
+	private final ArmFeedforward armFeedforward;
 
 	private Elbow() {
 		this.armFeedforward = ElbowConstants.ARM_FEEDFORWARD;
 		this.motor = new CANSparkMax(ElbowConstants.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless);
-		motor.getPIDController().setP(ElbowConstants.MOTOR_P);
-		motor.getPIDController().setI(ElbowConstants.MOTOR_I);
-		motor.getPIDController().setD(ElbowConstants.MOTOR_D);
+		motor.getPIDController().setP(ElbowConstants.P);
+		motor.getPIDController().setI(ElbowConstants.I);
+		motor.getPIDController().setD(ElbowConstants.D);
 	}
 
 	public static void init() {
@@ -40,7 +40,6 @@ public Rotation2d getVelocity() {
 		return Rotation2d.fromRotations(motor.getEncoder().getVelocity());
 	}
 
-
 	public void goToAngle(Rotation2d targetAngle) {
 		motor.getPIDController()
 			.setReference(
@@ -55,17 +54,17 @@ public boolean isAtAngle(Rotation2d targetAngle) {
 		return Math.abs(targetAngle.getDegrees() - motor.getEncoder().getPosition()) == 0;
 	}
 
-	public void stop() {
+	public void stayAtPosition() {
 		goToAngle(getPosition());
 	}
 
-
 	@Override
 	protected String getLogPath() {
-		return "";
+		return "Elbow/";
 	}
 
 	@Override
-	protected void subsystemPeriodic() {}
+	protected void subsystemPeriodic() {
+	}
 
-}
+}

src/main/java/training/subsystems/elbow/ElbowConstants.java

@@ -24,9 +24,9 @@ public enum PresetPositions {
 
 
 	public static final int MOTOR_ID = 5;
-	public static final int MOTOR_P = 1;
-	public static final int MOTOR_I = 0;
-	public static final int MOTOR_D = 0;
+	public static final int P = 1;
+	public static final int I = 0;
+	public static final int D = 0;
 
 	public static final int PID_SLOT = 0;
 
@@ -41,4 +41,4 @@ public enum PresetPositions {
 	public static final Rotation2d BACKWARD_ANGLE_LIMIT = Rotation2d.fromDegrees(-81);
 	public static final Rotation2d FORWARD_ANGLE_LIMIT = Rotation2d.fromDegrees(90);
 
-}
+}

src/main/java/training/subsystems/elbow/ElbowFactory.java

@@ -1,4 +1,4 @@
 package training.subsystems.elbow;
 
 public class ElbowFactory {
-}
+}

src/main/java/training/subsystems/elbow/ElbowNEO.java

@@ -6,7 +6,7 @@
 import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
 
-public class ElbowNEO implements IElbow {
+public class ElbowNEO {
 
 	private CANSparkMax motor;
 	private static ElbowNEO instance;
@@ -15,9 +15,9 @@ public class ElbowNEO implements IElbow {
 	private ElbowNEO() {
 		this.armFeedforward = ElbowConstants.ARM_FEEDFORWARD;
 		this.motor = new CANSparkMax(ElbowConstants.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless);
-		motor.getPIDController().setP(ElbowConstants.MOTOR_P);
-		motor.getPIDController().setI(ElbowConstants.MOTOR_I);
-		motor.getPIDController().setD(ElbowConstants.MOTOR_D);
+		motor.getPIDController().setP(ElbowConstants.P);
+		motor.getPIDController().setI(ElbowConstants.I);
+		motor.getPIDController().setD(ElbowConstants.D);
 	}
 
 	public void init() {
@@ -56,9 +56,8 @@ public boolean isAtAngle(Rotation2d targetAngle) {
 		return Math.abs(targetAngle.getDegrees() - motor.getEncoder().getPosition()) == 0;
 	}
 
-	public void stop() {
+	public void stayAtPosition() {
 		goToAngle(getPosition());
 	}
 
-
-}
+}

src/main/java/training/subsystems/elbow/ElbowSimulation.java

@@ -14,6 +14,7 @@ public class ElbowSimulation implements IElbow {
 	private SingleJointedArmSimulation elbowSimulation;
 	private ArmFeedforward armFeedforward;
 	private PIDController controller;
+	private PositionVoltage pos;
 
 	private ElbowSimulation() {
 		SingleJointedArmSim armSim = new SingleJointedArmSim(
@@ -35,6 +36,11 @@ private ElbowSimulation() {
 		elbowSimulation.applyConfiguration(config);
 	}
 
+	public void goToAngle(Rotation2d targetAngle) {
+		pos = new PositionVoltage(targetAngle.getRotations());
+		elbowSimulation.setControl(pos);
+	}
+
 	public Rotation2d getPosition() {
 		return elbowSimulation.getPosition();
 	}
@@ -43,14 +49,8 @@ public Rotation2d getVelocity() {
 		return elbowSimulation.getVelocity();
 	}
 
-
-	public void goToAngle(Rotation2d targetAngle) {
-		PositionVoltage pos = new PositionVoltage(targetAngle.getRotations());
-		elbowSimulation.setControl(pos);
-	}
-
-	public void stop() {
+	public void stayAtPosition() {
 		goToAngle(getPosition());
 	}
 
-}
+}

src/main/java/training/subsystems/elbow/SimulationElbowConstants.java

@@ -11,4 +11,4 @@ public class SimulationElbowConstants {
 
 	public static final double D = 0;
 
-}
+}

src/main/java/training/subsystems/roller/IRoller.java

@@ -2,14 +2,6 @@
 
 public interface IRoller {
 
-	void stop();
-
-	Roller getInstance();
-
 	void goToSpeed(double targetSpeed);
 
-	boolean isAtVelocity(double targetVelocity);
-
-	void init();
-
-}
+}

src/main/java/training/subsystems/roller/Roller.java

@@ -7,14 +7,14 @@
 
 public class Roller extends GBSubsystem {
 
-	private static CANSparkMax motor;
 	private static Roller instance;
+	private final CANSparkMax motor;
 
 	private Roller() {
 		motor = new CANSparkMax(RollerConstants.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless);
-		motor.getPIDController().setP(RollerConstants.MOTOR_P);
-		motor.getPIDController().setI(RollerConstants.MOTOR_I);
-		motor.getPIDController().setD(RollerConstants.MOTOR_D);
+		motor.getPIDController().setP(RollerConstants.P);
+		motor.getPIDController().setI(RollerConstants.I);
+		motor.getPIDController().setD(RollerConstants.D);
 	}
 
 
@@ -41,14 +41,13 @@ public void stop() {
 		motor.set(0);
 	}
 
-
 	@Override
 	protected String getLogPath() {
-		return "";
+		return "Roller/";
 	}
 
 	@Override
-	protected void subsystemPeriodic() {}
-
-}
+	protected void subsystemPeriodic() {
+	}
 
+}

src/main/java/training/subsystems/roller/RollerConstants.java

@@ -3,9 +3,8 @@
 public class RollerConstants {
 
 	public static final int MOTOR_ID = 22;
-	public static final int MOTOR_P = 1;
-	public static final int MOTOR_I = 0;
-	public static final int MOTOR_D = 0;
+	public static final int P = 1;
+	public static final int I = 0;
+	public static final int D = 0;
 
-
-}
+}

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

@@ -6,12 +6,8 @@ public interface IWrist {
 
 	void init();
 
-	void goToAngel(Rotation2d targetAngle);
-
-	boolean isAtAngle(Rotation2d targetAngle);
-
-	Wrist getInstance();
+	void goToAngle(Rotation2d targetAngle);
 
 	void stop();
 
-}
+}

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

@@ -8,14 +8,14 @@
 
 public class Wrist extends GBSubsystem {
 
-	private static CANSparkMax motor;
 	private static Wrist instance;
+	private final CANSparkMax motor;
 
 	private Wrist() {
 		motor = new CANSparkMax(WristConstant.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless);
-		motor.getPIDController().setP(WristConstant.MOTOR_P);
-		motor.getPIDController().setI(WristConstant.MOTOR_I);
-		motor.getPIDController().setD(WristConstant.MOTOR_D);
+		motor.getPIDController().setP(WristConstant.P);
+		motor.getPIDController().setI(WristConstant.I);
+		motor.getPIDController().setD(WristConstant.D);
 	}
 
 	public static Wrist getInstance() {
@@ -29,7 +29,7 @@ public static void init() {
 		}
 	}
 
-	public static void goToAngle(Rotation2d targetAngle) {
+	public void goToAngle(Rotation2d targetAngle) {
 		motor.getPIDController().setReference(targetAngle.getDegrees(), CANSparkBase.ControlType.kPosition);
 	}
 
@@ -41,14 +41,13 @@ public void stop() {
 		motor.set(0);
 	}
 
-
 	@Override
 	protected String getLogPath() {
-		return "Wrist";
+		return "Wrist/";
 	}
 
 	@Override
-	protected void subsystemPeriodic() {}
-
-}
+	protected void subsystemPeriodic() {
+	}
 
+}