From e12b953bb073acf93f3859be6c017c5998e9dc0c Mon Sep 17 00:00:00 2001
From: Yotam7 <76944434+Yotam7@users.noreply.github.com>
Date: Wed, 7 Aug 2024 17:13:48 +0300
Subject: [PATCH] Yotam- make elbow which works for both simulation and real,
 robot type constant, ElbowFactory

---
 src/main/java/subsystems/elbow/Elbow.java     | 32 +++++++++++++++++++
 src/main/java/subsystems/elbow/Factory.java   | 16 ++++++++++
 .../subsystems/elbow/neoElbow/NeoElbow.java   | 10 +-----
 .../simulationElbow/SimulationElbow.java      | 10 +-----
 src/main/java/training/Robot.java             |  8 +++++
 5 files changed, 58 insertions(+), 18 deletions(-)
 create mode 100644 src/main/java/subsystems/elbow/Elbow.java
 create mode 100644 src/main/java/subsystems/elbow/Factory.java

diff --git a/src/main/java/subsystems/elbow/Elbow.java b/src/main/java/subsystems/elbow/Elbow.java
new file mode 100644
index 0000000..bf9608f
--- /dev/null
+++ b/src/main/java/subsystems/elbow/Elbow.java
@@ -0,0 +1,32 @@
+package subsystems.elbow;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public class Elbow implements IElbow {
+
+    private IElbow iElbow;
+
+    public Elbow(){
+        iElbow =Factory.create();
+    }
+
+    @Override
+    public Rotation2d getAngle() {
+        return iElbow.getAngle();
+    }
+
+    @Override
+    public double getRPMVelocity() {
+        return iElbow.getRPMVelocity();
+    }
+
+    @Override
+    public void setPower(double power) {
+        iElbow.setPower(power);
+    }
+
+    @Override
+    public void goToPosition(Rotation2d position) {
+        iElbow.goToPosition(position);
+    }
+}
diff --git a/src/main/java/subsystems/elbow/Factory.java b/src/main/java/subsystems/elbow/Factory.java
new file mode 100644
index 0000000..d5ebe5c
--- /dev/null
+++ b/src/main/java/subsystems/elbow/Factory.java
@@ -0,0 +1,16 @@
+package subsystems.elbow;
+
+import subsystems.elbow.neoElbow.NeoElbow;
+import subsystems.elbow.simulationElbow.SimulationElbow;
+import training.Robot;
+
+public class Factory {
+
+    public static IElbow create(){
+        return switch (Robot.ROBOT_TYPE){
+            case REAL -> new NeoElbow();
+            case SIMULATION -> new SimulationElbow();
+        };
+    }
+
+}
diff --git a/src/main/java/subsystems/elbow/neoElbow/NeoElbow.java b/src/main/java/subsystems/elbow/neoElbow/NeoElbow.java
index b583c45..c9d5c95 100644
--- a/src/main/java/subsystems/elbow/neoElbow/NeoElbow.java
+++ b/src/main/java/subsystems/elbow/neoElbow/NeoElbow.java
@@ -9,23 +9,15 @@
 
 public class NeoElbow extends GBSubsystem implements IElbow {
 
-	private static NeoElbow instance;
 	private static CANSparkMax motor;
 
-	private NeoElbow() {
+	public NeoElbow() {
 		motor = new CANSparkMax(NeoElbowConstants.MOTOR_ID, RobotConstants.MOTOR_BRUSHLESS_TYPE);
 		motor.getPIDController().setP(ElbowConstants.KP);
 		motor.getPIDController().setI(ElbowConstants.KI);
 		motor.getPIDController().setD(ElbowConstants.KD);
 	}
 
-	public static NeoElbow getInstance() {
-		if (instance == null) {
-			instance = new NeoElbow();
-		}
-		return instance;
-	}
-
 	public Rotation2d getAngle() {
 		return Rotation2d.fromRotations(motor.getEncoder().getPosition() % 1);
 	}
diff --git a/src/main/java/subsystems/elbow/simulationElbow/SimulationElbow.java b/src/main/java/subsystems/elbow/simulationElbow/SimulationElbow.java
index a5487b7..ade4b37 100644
--- a/src/main/java/subsystems/elbow/simulationElbow/SimulationElbow.java
+++ b/src/main/java/subsystems/elbow/simulationElbow/SimulationElbow.java
@@ -10,11 +10,10 @@
 
 public class SimulationElbow implements IElbow {
 
-	private static SimulationElbow instance;
 	private final SingleJointedArmSim motor;
 	private final PIDController controller;
 
-	private SimulationElbow() {
+	public SimulationElbow() {
 		this.motor = new SingleJointedArmSim(
 			DCMotor.getFalcon500(SimulationElbowConstants.NUMBER_OF_MOTORS),
 			SimulationElbowConstants.GEAR_RATIO,
@@ -28,13 +27,6 @@ private SimulationElbow() {
 		this.controller = new PIDController(ElbowConstants.KP, ElbowConstants.KI, ElbowConstants.KD);
 	}
 
-	public SimulationElbow getInstance() {
-		if (instance == null) {
-			instance = new SimulationElbow();
-		}
-		return instance;
-	}
-
 	@Override
 	public Rotation2d getAngle() {
 		return Rotation2d.fromRadians(motor.getAngleRads());
diff --git a/src/main/java/training/Robot.java b/src/main/java/training/Robot.java
index f88c2e1..e4c01be 100644
--- a/src/main/java/training/Robot.java
+++ b/src/main/java/training/Robot.java
@@ -6,6 +6,14 @@
 
 public class Robot {
 
+	public enum RobotType{
+		SIMULATION,
+		REAL
+	}
+
+	public static final RobotType ROBOT_TYPE = RobotType.REAL;
+
+
 	// Enter your subsystems...
 	private final NeoElbow elbow;
 	private final Wrist wrist;