Nitay - made roller into a polymorphism system

src/main/java/training/Robot.java

@@ -1,7 +1,7 @@
 package training;
 
 import training.subsystems.ArmSubsystems.elbow.Elbow;
-import training.subsystems.ArmSubsystems.roller.Roller;
+//import training.subsystems.ArmSubsystems.roller.Roller;
 import training.subsystems.ArmSubsystems.wrist.Wrist;
 
 public class Robot {
@@ -11,14 +11,14 @@ public class Robot {
 	public static final Robot.robotType ROBOT_TYPE = Robot.robotType.SIMULATION;
 	private final Elbow elbow;
 	private final Wrist wrist;
-	private final Roller roller;
+//	private final Roller roller;
 
 
 	public Robot() {
 		// Boot your subsystems...
 		this.elbow = new Elbow();
 		this.wrist = new Wrist();
-		this.roller = new Roller();
+//		this.roller = new Roller();
 	}
 
 
@@ -31,9 +31,9 @@ public Elbow getElbow() {
 		return elbow;
 	}
 
-	public Roller getRoller() {
-		return roller;
-	}
+//	public Roller getRoller() {
+//		return roller;
+//	}
 
 	public Wrist getWrist() {
 		return wrist;

src/main/java/training/TrainingRobotManager.java

@@ -3,8 +3,8 @@
 import training.commands.ArmCommands.elbowCommands.ElbowStayInPlace;
 import training.commands.ArmCommands.elbowCommands.MoveElbow;
 import training.commands.ArmCommands.wristCommands.MoveWrist;
-import training.commands.ArmCommands.rollerCommands.RollClockwise;
-import training.commands.ArmCommands.rollerCommands.RollCounterClockwise;
+//import training.commands.ArmCommands.rollerCommands.RollClockwise;
+//import training.commands.ArmCommands.rollerCommands.RollCounterClockwise;
 import training.commands.ArmCommands.wristCommands.StopWrist;
 import training.subsystems.ArmSubsystems.elbow.ElbowConstants;
 import training.subsystems.ArmSubsystems.wrist.WristConstants;
@@ -27,8 +27,8 @@ public void trainingInit() {
 		smartJoystick.Y.onTrue(new MoveWrist(WristConstants.CLIMBING_POSITION, robot.getWrist()));
 		smartJoystick.X.onTrue(new MoveWrist(WristConstants.STARTING_POSITION, robot.getWrist()));
 
-		smartJoystick.R1.whileTrue(new RollClockwise(robot.getRoller()));
-		smartJoystick.R2.whileTrue(new RollCounterClockwise(robot.getRoller()));
+//		smartJoystick.R1.whileTrue(new RollClockwise(robot.getRoller()));
+//		smartJoystick.R2.whileTrue(new RollCounterClockwise(robot.getRoller()));
 
 		robot.getElbow().setDefaultCommand(new ElbowStayInPlace(robot.getElbow()));
 		robot.getWrist().setDefaultCommand(new StopWrist(robot.getWrist()));

src/main/java/training/commands/ArmCommands/elbowCommands/ElbowStayInPlace.java

@@ -4,7 +4,6 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import training.subsystems.ArmSubsystems.elbow.Elbow;
 
-import java.lang.module.ModuleDescriptor;
 
 public class ElbowStayInPlace extends Command {
 

src/main/java/training/commands/ArmCommands/rollerCommands/RollClockwise.java

@@ -10,7 +10,6 @@ public class RollClockwise extends Command {
 	public RollClockwise(Roller roller) {
 		this.roller = roller;
 		addRequirements(roller);
-
 	}
 
 	@Override

src/main/java/training/commands/ArmCommands/wristCommands/MoveWrist.java

@@ -14,7 +14,6 @@ public MoveWrist(Rotation2d targetPosition, Wrist wrist) {
 		this.wrist = wrist;
 		this.targetPosition = targetPosition;
 		addRequirements(wrist);
-
 	}
 
 	@Override

src/main/java/training/commands/ArmCommands/wristCommands/StopWrist.java

@@ -10,7 +10,6 @@ public class StopWrist extends Command {
 	public StopWrist(Wrist wrist) {
 		this.wrist = wrist;
 		addRequirements(wrist);
-
 	}
 
 	@Override

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

@@ -34,7 +34,7 @@ public static Rotation2d angleDistance(Rotation2d currentAngle, Rotation2d targe
 
 	public boolean isAtTargetPosition(Rotation2d targetAngle, Rotation2d positionTolerance, Rotation2d velocityTolerance) {
 		return Math.abs(angleDistance(inputs.position, targetAngle).getDegrees()) <= positionTolerance.getDegrees()
-			&& inputs.velocity.getRotations() <= velocityTolerance.getRotations();
+			&& inputs.position.getRotations() <= velocityTolerance.getRotations();
 	}
 
 	public void goToPosition(Rotation2d targetPosition) {

src/main/java/training/subsystems/ArmSubsystems/elbow/IElbow.java

@@ -1,6 +1,7 @@
 package training.subsystems.ArmSubsystems.elbow;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import training.subsystems.ArmSubsystems.roller.RollerInputsAutoLogged;
 
 public interface IElbow {
 
@@ -11,7 +12,4 @@ public interface IElbow {
 	void setPower(double power);
 
 	void updateInputs(ElbowInputsAutoLogged inputs);
-
-	void stayAtPosition();
-
 }

src/main/java/training/subsystems/ArmSubsystems/elbow/simulation/SimulationConstants.java

@@ -8,7 +8,7 @@ public class SimulationConstants {
 
 	public static final int ID = 0;
 
-	public static final double P_VALUE = 100;
+	public static final double P_VALUE = 150;
 	public static final double I_VALUE = 0;
 	public static final double D_VALUE = 0;
 	public static final PIDController PID_CONTROLLER = new PIDController(P_VALUE, I_VALUE, D_VALUE);

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

@@ -0,0 +1,13 @@
+package training.subsystems.ArmSubsystems.roller;
+
+import training.subsystems.ArmSubsystems.elbow.ElbowInputsAutoLogged;
+
+public interface IRoller {
+
+	public void setPower(double power);
+
+	public void setVoltage(double voltage);
+
+	public void updateInputs(RollerInputsAutoLogged inputs);
+
+}

src/main/java/training/subsystems/ArmSubsystems/roller/NeoRoller/NeoConstants.java

@@ -0,0 +1,8 @@
+package training.subsystems.ArmSubsystems.roller.NeoRoller;
+
+
+public class NeoConstants {
+
+	protected static final int ID = 22;
+
+}

src/main/java/training/subsystems/ArmSubsystems/roller/NeoRoller/NeoRoller.java

@@ -0,0 +1,31 @@
+package training.subsystems.ArmSubsystems.roller.NeoRoller;
+
+import com.revrobotics.CANSparkLowLevel;
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.math.geometry.Rotation2d;
+import training.subsystems.ArmSubsystems.roller.IRoller;
+import training.subsystems.ArmSubsystems.roller.RollerInputsAutoLogged;
+
+public class NeoRoller implements IRoller {
+
+	private final CANSparkMax motor;
+
+
+	public NeoRoller() {
+		this.motor = new CANSparkMax(NeoConstants.ID, CANSparkLowLevel.MotorType.kBrushless);
+	}
+
+	public void setPower(double power) {
+		motor.set(power);
+	}
+
+	public void setVoltage(double voltage) {
+		motor.setVoltage(voltage);
+	}
+
+	public void updateInputs(RollerInputsAutoLogged inputs) {
+		inputs.position= Rotation2d.fromRotations(motor.getEncoder().getPosition());
+		inputs.velocity=Rotation2d.fromRotations(motor.getEncoder().getVelocity());
+	}
+
+}

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

@@ -1,16 +1,18 @@
 package training.subsystems.ArmSubsystems.roller;
 
-import com.revrobotics.CANSparkLowLevel;
-import com.revrobotics.CANSparkMax;
 import edu.wpi.first.math.geometry.Rotation2d;
+import training.subsystems.ArmSubsystems.elbow.ElbowInputsAutoLogged;
+import training.subsystems.ArmSubsystems.elbow.IElbow;
 import utils.GBSubsystem;
 
 public class Roller extends GBSubsystem {
 
-	private final CANSparkMax motor;
+	private final IRoller motor;
+	private final RollerInputsAutoLogged inputs;
 
 	public Roller() {
-		this.motor = new CANSparkMax(RollerConstants.ID, CANSparkLowLevel.MotorType.kBrushless);
+		this.motor=RollerFactory.create();
+		this.inputs=new RollerInputsAutoLogged();
 	}
 
 	@Override
@@ -22,19 +24,22 @@ protected String getLogPath() {
 	protected void subsystemPeriodic() {}
 
 	public void rollClockwise() {
-		motor.set(RollerConstants.DEFAULT_CLOCKWISE_POWER);
+		motor.setPower(RollerConstants.DEFAULT_CLOCKWISE_POWER);
 	}
 
 	public void rollCounterClockwise() {
-		motor.set(RollerConstants.DEFAULT_COUNTER_CLOCKWISE_POWER);
+		motor.setPower(RollerConstants.DEFAULT_COUNTER_CLOCKWISE_POWER);
 	}
 
 	public void stop() {
-		motor.set(0);
+		motor.setPower(0);
 	}
 
 	public Rotation2d getVelocity() {
-		return Rotation2d.fromRotations(motor.getEncoder().getVelocity());
+		return inputs.velocity;
+	}
+	public Rotation2d getPosition() {
+		return inputs.position;
 	}
 
 }

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

@@ -2,10 +2,8 @@
 
 public class RollerConstants {
 
-	protected static final int ID = 22;
 
 	protected static final double DEFAULT_CLOCKWISE_POWER = 0.3;
-
 	protected static final double DEFAULT_COUNTER_CLOCKWISE_POWER = -0.3;
 
 }

src/main/java/training/subsystems/ArmSubsystems/roller/RollerFactory.java

@@ -0,0 +1,16 @@
+package training.subsystems.ArmSubsystems.roller;
+
+import training.Robot;
+import training.subsystems.ArmSubsystems.roller.NeoRoller.NeoRoller;
+import training.subsystems.ArmSubsystems.roller.simulationRoller.SimulationRoller;
+
+public class RollerFactory {
+
+	public static IRoller create() {
+		return switch (Robot.ROBOT_TYPE) {
+			case SYNCOPA -> new NeoRoller();
+			case SIMULATION -> new SimulationRoller();
+		};
+	}
+
+}

src/main/java/training/subsystems/ArmSubsystems/roller/RollerInputs.java

@@ -0,0 +1,13 @@
+package training.subsystems.ArmSubsystems.roller;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.AutoLog;
+
+@AutoLog
+public class RollerInputs {
+
+	public Rotation2d position;
+	public Rotation2d velocity;
+	public double voltage;
+
+}

src/main/java/training/subsystems/ArmSubsystems/roller/simulationRoller/SimulationConstants.java

@@ -0,0 +1,13 @@
+package training.subsystems.ArmSubsystems.roller.simulationRoller;
+
+public class SimulationConstants {
+
+	protected static final int ID = 22;
+	public static final int NUMBER_OF_MOTORS = 1;
+
+	public static final double GEAR_RATIO = 1;
+
+	public static final double MOMENT_OF_INERTIA = 1;
+
+
+}

src/main/java/training/subsystems/ArmSubsystems/roller/simulationRoller/SimulationRoller.java

@@ -0,0 +1,39 @@
+package training.subsystems.ArmSubsystems.roller.simulationRoller;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import training.GlobalConstants;
+import training.subsystems.ArmSubsystems.elbow.ElbowInputsAutoLogged;
+import training.subsystems.ArmSubsystems.roller.IRoller;
+import training.subsystems.ArmSubsystems.roller.RollerInputsAutoLogged;
+
+public class SimulationRoller implements IRoller {
+
+	private final DCMotorSim motor;
+
+
+	public SimulationRoller() {
+		this.motor = new DCMotorSim(
+			DCMotor.getNEO(SimulationConstants.NUMBER_OF_MOTORS),
+			SimulationConstants.GEAR_RATIO,
+			SimulationConstants.MOMENT_OF_INERTIA
+		);
+	}
+
+	public void setPower(double power) {
+		setVoltage(power * GlobalConstants.MAX_BATTERY_VOLTAGE);
+	}
+
+	public void setVoltage(double voltage) {
+		motor.setInputVoltage(voltage);
+	}
+
+	@Override
+	public void updateInputs(RollerInputsAutoLogged inputs) {
+		inputs.position= Rotation2d.fromRotations(motor.getAngularPositionRotations());
+		inputs.velocity=Rotation2d.fromRadians(motor.getAngularVelocityRadPerSec());
+	}
+
+
+}