Finished Herman's and Kedem's CR expect unanswered questions and I ne…

…ed to Log some stuff. Splitted the ElbowConstants class to neo and sim, added constructor to Elbow that uses the factory and added some more functions.
GreenBlitz · Aug 7, 2024 · 94071ca · 94071ca
1 parent cbefacb
commit 94071ca
Show file tree

Hide file tree

Showing 8 changed files with 131 additions and 103 deletions.
diff --git a/src/main/java/training/commands/Arm/Wrist/MoveWrist.java b/src/main/java/training/commands/Arm/Wrist/MoveWrist.java
@@ -17,7 +17,7 @@ public MoveWrist(Rotation2d targetPosition) {
 
 	@Override
 	public void initialize() {
-		wrist.goTo(targetPosition);
+		wrist.moveToAngle(targetPosition);
 	}
 
 	@Override

diff --git a/src/main/java/training/subsystems/Arm/Elbow/Elbow.java b/src/main/java/training/subsystems/Arm/Elbow/Elbow.java
@@ -5,9 +5,12 @@
 
 public class Elbow extends GBSubsystem {
 
-	protected static Elbow instance;
 	private IElbow iElbow;
 
+	public Elbow(){
+		this.iElbow = ElbowFactory.create();
+	}
+
 	@Override
 	protected String getLogPath() {
 		return null;
@@ -16,12 +19,7 @@ protected String getLogPath() {
 	@Override
 	protected void subsystemPeriodic() {}
 
-	public static Elbow getInstance() {
-		if (instance == null) {
-			instance = new Elbow();
-		}
-		return instance;
-	}
+
 
 	public void setPower(double power) {
 		iElbow.setPower(power);
@@ -31,4 +29,14 @@ public boolean isAtPosition(Rotation2d target) {
 		return Math.abs(iElbow.getPosition().getDegrees() - target.getDegrees()) <= ElbowConstants.TOLERANCE;
 	}
 
+	public void moveToPosition(Rotation2d targetPosition) {
+		iElbow.moveToPosition(targetPosition);
+	}
+
+	public void stayAtPosition() {
+		moveToPosition(iElbow.getPosition());
+	}
+
+
+
 }
diff --git a/src/main/java/training/subsystems/Arm/Elbow/ElbowConstants.java b/src/main/java/training/subsystems/Arm/Elbow/ElbowConstants.java
@@ -1,78 +1,6 @@
 package training.subsystems.Arm.Elbow;
 
-import edu.wpi.first.math.controller.ArmFeedforward;
-import edu.wpi.first.math.geometry.Rotation2d;
-
 public class ElbowConstants {
 
-	protected static final int MOTOR_ID = 0;
-
-	protected static final double TOLERANCE = 5;
-
-	protected static final Rotation2d ELBOW_START_POSITION = Rotation2d.fromDegrees(0);
-
-	protected static final Rotation2d ELBOW_CLIMBING_POSITION = Rotation2d.fromDegrees(180);
-
-	protected static final ArmFeedforward ARM_FEEDFORWARD = new ArmFeedforward(
-		ElbowConstants.KS,
-		ElbowConstants.KG,
-		ElbowConstants.KV,
-		ElbowConstants.KA
-	);
-
-	protected static final int PID_SLOT = 0;
-
-	protected static final double P = 89;
-
-	protected static final double I = 76;
-
-	protected static final double D = 45;
-
-	protected static final double KS = 0;
-
-	protected static final double KG = 0.12;
-
-	protected static final double KV = 0;
-
-	protected static final double KA = 0;
-
-
-	public static final double ELBOW_GEAR_RATIO = (1 / (28.0 * (60.0 / 16.0)));
-
-
-	public static final int NUMBER_OF_MOTORS = 1;
-
-	public enum PresetPositions {
-
-		SAFE(Rotation2d.fromDegrees(-67)),
-		INTAKE(Rotation2d.fromDegrees(-76)),
-		SCORE(Rotation2d.fromDegrees(55)),
-		STARTING(Rotation2d.fromDegrees(0)),
-		TRANSFER(Rotation2d.fromDegrees(-80));
-
-		public final Rotation2d ANGLE;
-
-		PresetPositions(Rotation2d angle) {
-			this.ANGLE = angle;
-		}
-
-	}
-
-	public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(-82);
-
-	public static final double ARM_LENGTH = 0.44;
-
-	public static final double ARM_DISTANCE_FROM_CENTER = 0.1;
-
-	public static final double ARM_MASS_KG = 0.44;
-
-	public static final Rotation2d BACKWARD_ANGLE_LIMIT = Rotation2d.fromDegrees(-81);
-
-	public static final Rotation2d FORWARD_ANGLE_LIMIT = Rotation2d.fromDegrees(90);
-
-
-	public static final double GEAR_RATIO = 1 / (28.0 * (60.0 / 16.0));
-
-	public static final int CURRENT_LIMIT = 40;
-
+    protected static final double TOLERANCE = 5;
 }
diff --git a/src/main/java/training/subsystems/Arm/Elbow/NeoElbow.java b/src/main/java/training/subsystems/Arm/Elbow/NeoElbow.java
@@ -11,12 +11,12 @@ public class NeoElbow implements IElbow {
 	private static NeoElbow instance;
 
 	public NeoElbow() {
-		this.motor = new CANSparkMax(ElbowConstants.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless);
-		motor.getEncoder().setPositionConversionFactor(ElbowConstants.ELBOW_GEAR_RATIO);
-		motor.getPIDController().setP(ElbowConstants.P);
-		motor.getPIDController().setI(ElbowConstants.I);
-		motor.getPIDController().setD(ElbowConstants.D);
-		motor.getEncoder().setPosition(ElbowConstants.ELBOW_START_POSITION.getDegrees());
+		this.motor = new CANSparkMax(NeoElbowConstants.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless);
+		motor.getEncoder().setPositionConversionFactor(NeoElbowConstants.ELBOW_GEAR_RATIO);
+		motor.getPIDController().setP(NeoElbowConstants.P);
+		motor.getPIDController().setI(NeoElbowConstants.I);
+		motor.getPIDController().setD(NeoElbowConstants.D);
+		motor.getEncoder().setPosition(NeoElbowConstants.ELBOW_START_POSITION.getDegrees());
 	}
 
 	public static NeoElbow getInstance() {
@@ -32,8 +32,8 @@ public void moveToPosition(Rotation2d position) {
 			.setReference(
 				position.getDegrees(),
 				CANSparkBase.ControlType.kPosition,
-				ElbowConstants.PID_SLOT,
-				ElbowConstants.ARM_FEEDFORWARD.calculate(getPosition().getRadians(), getVelocity().getRotations())
+				NeoElbowConstants.PID_SLOT,
+				NeoElbowConstants.ARM_FEEDFORWARD.calculate(getPosition().getRadians(), getVelocity().getRotations())
 			);
 	}
 
@@ -59,7 +59,7 @@ public void setPower(double power) {
 
 
 	public boolean isAtPosition(Rotation2d target) {
-		return Math.abs(getPosition().getDegrees() - target.getDegrees()) <= ElbowConstants.TOLERANCE;
+		return Math.abs(getPosition().getDegrees() - target.getDegrees()) <= NeoElbowConstants.TOLERANCE;
 	}
 
 }
diff --git a/src/main/java/training/subsystems/Arm/Elbow/NeoElbowConstants.java b/src/main/java/training/subsystems/Arm/Elbow/NeoElbowConstants.java
@@ -0,0 +1,41 @@
+package training.subsystems.Arm.Elbow;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public class NeoElbowConstants {
+
+	protected static final int MOTOR_ID = 0;
+
+	protected static final double TOLERANCE = 5;
+
+	protected static final Rotation2d ELBOW_START_POSITION = Rotation2d.fromDegrees(0);
+
+	protected static final Rotation2d ELBOW_CLIMBING_POSITION = Rotation2d.fromDegrees(180);
+
+	protected static final ArmFeedforward ARM_FEEDFORWARD = new ArmFeedforward(
+		NeoElbowConstants.KS,
+		NeoElbowConstants.KG,
+		NeoElbowConstants.KV,
+		NeoElbowConstants.KA
+	);
+
+	protected static final int PID_SLOT = 0;
+
+	protected static final double P = 89;
+
+	protected static final double I = 76;
+
+	protected static final double D = 45;
+
+	protected static final double KS = 0;
+
+	protected static final double KG = 0.12;
+
+	protected static final double KV = 0;
+
+	protected static final double KA = 0;
+
+	public static final double ELBOW_GEAR_RATIO = (1 / (28.0 * (60.0 / 16.0)));
+
+}
diff --git a/src/main/java/training/subsystems/Arm/Elbow/SimulationElbow.java b/src/main/java/training/subsystems/Arm/Elbow/SimulationElbow.java
@@ -12,9 +12,8 @@ public class SimulationElbow implements IElbow {
 
 	private static SimulationElbow instance;
 	private final SingleJointedArmSimulation elbowSimulation;
-	private final SingleJointedArmSim armSim;
 	private final PIDController controller;
-	private TalonFXConfiguration config;
+	private final TalonFXConfiguration config;
 
 	public static SimulationElbow getInstance() {
 		if (instance == null) {
@@ -24,17 +23,17 @@ public static SimulationElbow getInstance() {
 	}
 
 	public SimulationElbow() {
-		this.armSim = new SingleJointedArmSim(
-			DCMotor.getFalcon500(ElbowConstants.NUMBER_OF_MOTORS),
-			ElbowConstants.GEAR_RATIO,
-			SingleJointedArmSim.estimateMOI(ElbowConstants.ARM_LENGTH, ElbowConstants.ARM_MASS_KG),
-			ElbowConstants.ARM_LENGTH,
-			ElbowConstants.BACKWARD_ANGLE_LIMIT.getRadians(),
-			ElbowConstants.FORWARD_ANGLE_LIMIT.getRadians(),
+		final SingleJointedArmSim armSim = new SingleJointedArmSim(
+			DCMotor.getFalcon500(SimulationElbowConstants.NUMBER_OF_MOTORS),
+				SimulationElbowConstants.GEAR_RATIO,
+			SingleJointedArmSim.estimateMOI(SimulationElbowConstants.ARM_LENGTH, SimulationElbowConstants.ARM_MASS_KG),
+				SimulationElbowConstants.ARM_LENGTH,
+				SimulationElbowConstants.BACKWARD_ANGLE_LIMIT.getRadians(),
+				SimulationElbowConstants.FORWARD_ANGLE_LIMIT.getRadians(),
 			false,
-			ElbowConstants.PresetPositions.STARTING.ANGLE.getRadians()
+				SimulationElbowConstants.PresetPositions.STARTING.ANGLE.getRadians()
 		);
-		this.controller = new PIDController(ElbowConstants.P, ElbowConstants.I, ElbowConstants.D);
+		this.controller = new PIDController(SimulationElbowConstants.P, SimulationElbowConstants.I, SimulationElbowConstants.D);
 		this.elbowSimulation = new SingleJointedArmSimulation(armSim);
 		config = new TalonFXConfiguration();
 		config.Slot0.kP = controller.getP();
@@ -71,7 +70,7 @@ public Rotation2d getVelocity() {
 
 
 	public boolean isAtPosition(Rotation2d target) {
-		return Math.abs(elbowSimulation.getPosition().getDegrees() - target.getDegrees()) <= ElbowConstants.TOLERANCE;
+		return Math.abs(elbowSimulation.getPosition().getDegrees() - target.getDegrees()) <= SimulationElbowConstants.TOLERANCE;
 	}
 
 }
diff --git a/src/main/java/training/subsystems/Arm/Elbow/SimulationElbowConstants.java b/src/main/java/training/subsystems/Arm/Elbow/SimulationElbowConstants.java
@@ -0,0 +1,52 @@
+package training.subsystems.Arm.Elbow;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public class SimulationElbowConstants {
+
+    protected static final double P = 89;
+
+    protected static final double I = 76;
+
+    protected static final double D = 45;
+
+    protected static final double TOLERANCE = 5;
+
+
+
+
+    public static final int NUMBER_OF_MOTORS = 1;
+
+    public enum PresetPositions {
+
+        SAFE(Rotation2d.fromDegrees(-67)),
+        INTAKE(Rotation2d.fromDegrees(-76)),
+        SCORE(Rotation2d.fromDegrees(55)),
+        STARTING(Rotation2d.fromDegrees(0)),
+        TRANSFER(Rotation2d.fromDegrees(-80));
+
+        public final Rotation2d ANGLE;
+
+        PresetPositions(Rotation2d angle) {
+            this.ANGLE = angle;
+        }
+
+    }
+
+    public static final Rotation2d MINIMUM_ANGLE = Rotation2d.fromDegrees(-82);
+
+    public static final double ARM_LENGTH = 0.44;
+
+    public static final double ARM_DISTANCE_FROM_CENTER = 0.1;
+
+    public static final double ARM_MASS_KG = 0.44;
+
+    public static final Rotation2d BACKWARD_ANGLE_LIMIT = Rotation2d.fromDegrees(-81);
+
+    public static final Rotation2d FORWARD_ANGLE_LIMIT = Rotation2d.fromDegrees(90);
+
+
+    public static final double GEAR_RATIO = 1 / (28.0 * (60.0 / 16.0));
+
+    public static final int CURRENT_LIMIT = 40;
+}
diff --git a/src/main/java/training/subsystems/Arm/Wrist/Wrist.java b/src/main/java/training/subsystems/Arm/Wrist/Wrist.java
@@ -42,7 +42,7 @@ public static Wrist getInstance() {
 		return instance;
 	}
 
-	public void goTo(Rotation2d targetPosition) {
+	public void moveToAngle(Rotation2d targetPosition) {
 		motor.selectProfileSlot(WristConstants.PID_SLOT, 0);
 		motor.set(ControlMode.Position, targetPosition.getRotations());
 	}