Aviv elbow

src/main/java/training/Robot.java

@@ -2,7 +2,13 @@
 
 public class Robot {
 
+	public static final RobotType ROBOT_TYPE = RobotType.Simulation;
+
 	// Enter your subsystems...
+	public enum RobotType {
+		Simulation,
+		Real,
+	}
 
 	public Robot() {
 		// Boot your subsystems...

src/main/java/training/TrainingRobotManager.java

@@ -1,16 +1,28 @@
 package training;
 
+import edu.wpi.first.math.geometry.Rotation2d;
+import training.subsystems.arm.elbow.Elbow;
 import utils.DefaultRobotManager;
+import utils.KeyboardController;
 
 public class TrainingRobotManager extends DefaultRobotManager {
 
 	private Robot robot;
+	private KeyboardController keyboardController;
+	private Elbow elbow;
 
 	@Override
 	public void trainingInit() {
 		this.robot = new Robot();
+		this.keyboardController = new KeyboardController();
+
+		keyboardController.A.onTrue(elbow.getCommmands().moveToPosition(Rotation2d.fromDegrees(0)));
+		keyboardController.W.onTrue(elbow.getCommmands().moveToPosition(Rotation2d.fromDegrees(324)));
+		keyboardController.D.onTrue(elbow.getCommmands().moveToPosition(Rotation2d.fromDegrees(20)));
+		keyboardController.S.onTrue(elbow.getCommmands().moveToPosition(Rotation2d.fromDegrees(80)));
 	}
 
+
 	@Override
 	public void trainingPeriodic() {
 		// add stuff...

src/main/java/training/subsystems/arm/elbow/CommandsBuilder.java

@@ -0,0 +1,24 @@
+package training.subsystems.arm.elbow;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+
+public class CommandsBuilder {
+
+	private final Elbow elbow;
+
+	public CommandsBuilder(Elbow elbow) {
+		this.elbow = elbow;
+	}
+
+	public Command moveToPosition(Rotation2d angle) {
+		return new FunctionalCommand(
+			() -> elbow.moveToAngle(angle),
+			() -> {},
+			interrupted -> elbow.stayInPlace(),
+			() -> elbow.isAtAngle(angle)
+		);
+	}
+
+}

src/main/java/training/subsystems/arm/elbow/Constants.java

@@ -0,0 +1,30 @@
+package training.subsystems.arm.elbow;
+
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+class Constants {
+
+	static int MOTOR_ID = 1;
+
+	static Rotation2d TOLERANCE = Rotation2d.fromDegrees(10);
+
+	static final double GEAR_RATIO = 1;
+
+	static final double KP = 0;
+
+	static final double KI = 0;
+
+	static final double KD = 0;
+
+	static final double KG = 0;
+
+	static final double KS = 0;
+
+	static final double KV = 0;
+
+	static final double BATTERY_VOLTAGE = 12;
+	static final ArmFeedforward ELBOW_FEEDFORWARD = new ArmFeedforward(KG, KS, KV);
+
+}

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

@@ -0,0 +1,53 @@
+package training.subsystems.arm.elbow;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import utils.GBSubsystem;
+
+public class Elbow extends GBSubsystem {
+
+	@Override
+	protected String getLogPath() {
+		return null;
+	}
+
+	@Override
+	protected void subsystemPeriodic() {}
+
+	private CommandsBuilder commmands;
+	private IElbow iElbow;
+
+
+	public Elbow() {
+		this.commmands = new CommandsBuilder(this);
+		this.iElbow = ElbowFactory.create();
+	}
+
+	public CommandsBuilder getCommmands() {
+		return this.commmands;
+	}
+
+	public Rotation2d getCurrentAngle() {
+		return iElbow.getCurrentAngle();
+	}
+
+	public void setPower(double power) {
+		iElbow.setPower(power);
+	}
+
+	public boolean isAtAngle(Rotation2d angle) {
+		return Math.abs((getCurrentAngle().getDegrees() - angle.getDegrees())) <= Constants.TOLERANCE.getDegrees();
+	}
+
+	public Rotation2d getCurrentVelocity() {
+		return iElbow.getCurrentVelocity();
+	}
+
+	public void moveToAngle(Rotation2d angle) {
+		iElbow.moveToAngle(angle);
+	}
+
+	public void stayInPlace() {
+		iElbow.stayInPlace();
+	}
+
+}

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

@@ -0,0 +1,14 @@
+package training.subsystems.arm.elbow;
+
+import training.Robot;
+
+public class ElbowFactory {
+
+	public static IElbow create() {
+		return switch (Robot.ROBOT_TYPE) {
+			case Real -> new NEOElbow();
+			case Simulation -> new SimulationElbow();
+		};
+	}
+
+}

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

@@ -0,0 +1,17 @@
+package training.subsystems.arm.elbow;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public interface IElbow {
+
+	public Rotation2d getCurrentAngle();
+
+	public void setPower(double power);
+
+	public Rotation2d getCurrentVelocity();
+
+	public void moveToAngle(Rotation2d angle);
+
+	public void stayInPlace();
+
+}

src/main/java/training/subsystems/arm/elbow/NEOElbow.java

@@ -0,0 +1,48 @@
+package training.subsystems.arm.elbow;
+
+import com.revrobotics.CANSparkBase;
+import com.revrobotics.CANSparkLowLevel;
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public class NEOElbow implements IElbow {
+
+	private final CANSparkMax motor;
+
+
+	public NEOElbow() {
+		this.motor = new CANSparkMax(Constants.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless);
+		motor.getEncoder().setPositionConversionFactor(Constants.GEAR_RATIO);
+		motor.getPIDController().setP(Constants.KP);
+		motor.getPIDController().setI(Constants.KI);
+		motor.getPIDController().setD(Constants.KD);
+	}
+
+	public Rotation2d getCurrentAngle() {
+		return Rotation2d.fromRotations(motor.getEncoder().getPosition());
+	}
+
+	public void setPower(double power) {
+		motor.set(power);
+	}
+
+
+	public Rotation2d getCurrentVelocity() {
+		return Rotation2d.fromRotations(motor.getEncoder().getVelocity());
+	}
+
+	public void moveToAngle(Rotation2d angle) {
+		motor.getPIDController()
+			.setReference(
+				angle.getRotations(),
+				CANSparkBase.ControlType.kPosition,
+				0,
+				Constants.ELBOW_FEEDFORWARD.calculate(getCurrentAngle().getRotations(), getCurrentVelocity().getRotations())
+			);
+	}
+
+	public void stayInPlace() {
+		moveToAngle(Rotation2d.fromRotations(getCurrentAngle().getRotations()));
+	}
+
+}

src/main/java/training/subsystems/arm/elbow/SimulationElbow.java

@@ -0,0 +1,60 @@
+package training.subsystems.arm.elbow;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import edu.wpi.first.math.system.plant.DCMotor;
+
+public class SimulationElbow implements IElbow {
+
+	private final SingleJointedArmSim elbowSimulation;
+	private final PIDController controller;
+
+	public SimulationElbow() {
+		elbowSimulation = 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,
+			SimulationElbowConstants.PresetPositions.STARTING.ANGLE.getRadians()
+		);
+		controller = SimulationElbowConstants.SIMULATION_PID;
+	}
+
+	@Override
+	public Rotation2d getCurrentAngle() {
+		return Rotation2d.fromRadians(elbowSimulation.getAngleRads());
+	}
+
+	@Override
+	public void setPower(double power) {
+		elbowSimulation.setInputVoltage(power * Constants.BATTERY_VOLTAGE);
+	}
+
+	@Override
+	public Rotation2d getCurrentVelocity() {
+		return Rotation2d.fromRotations(elbowSimulation.getVelocityRadPerSec() / SimulationElbowConstants.FULL_CIRCLE_RAD);
+	}
+
+	private void setVoltage(double voltage) {
+		double limited_voltage = Math.min((Math.max(voltage, -Constants.BATTERY_VOLTAGE)), Constants.BATTERY_VOLTAGE);
+		elbowSimulation.setInputVoltage(limited_voltage);
+	}
+
+
+	public void moveToAngle(Rotation2d angle) {
+		setVoltage(controller.calculate(elbowSimulation.getAngleRads(), angle.getRadians()));
+	}
+
+	@Override
+	public void stayInPlace() {
+		moveToAngle(getCurrentAngle());
+	}
+
+}
+

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

@@ -0,0 +1,60 @@
+package training.subsystems.arm.elbow;
+
+
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.controller.PIDController;
+
+public class SimulationElbowConstants {
+
+
+	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 int NUMBER_OF_MOTORS = 1;
+
+
+	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 Pair<
+		Rotation2d,
+		Rotation2d> SHOOTER_COLLISION_RANGE = new Pair<>(BACKWARD_ANGLE_LIMIT, PresetPositions.SAFE.ANGLE);
+
+	public static final Translation3d ELBOW_POSITION_RELATIVE_TO_ROBOT = new Translation3d(-0.1, 0, 0.6);
+
+	public static final double GEAR_RATIO = 1 / (28.0 * (60.0 / 16.0));
+
+	public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(3);
+
+	public static final int CURRENT_LIMIT = 40;
+
+	public static final PIDController SIMULATION_PID = new PIDController(Constants.KP, Constants.KI, Constants.KD);
+
+	public static final double FULL_CIRCLE_RAD = 2 * Math.PI;;
+
+}
+