Tomer-arm

src/main/java/training/GeneralConstants.java

@@ -0,0 +1,7 @@
+package training;
+
+public class GeneralConstants {
+
+	public static RobotTypes robotTypes = RobotTypes.SIMULATION;
+
+}

src/main/java/training/RobotTypes.java

@@ -0,0 +1,8 @@
+package training;
+
+public enum RobotTypes {
+
+	SIMULATION,
+	SYNCOPA;
+
+}

src/main/java/training/TrainingRobotManager.java

@@ -1,6 +1,11 @@
 package training;
 
+import edu.wpi.first.math.geometry.Rotation2d;
+import training.commands.elbow.MoveElbowToAngle;
+import training.subsystems.RobotArm.elbow.Elbow;
 import utils.DefaultRobotManager;
+import utils.joysticks.JoystickPorts;
+import utils.joysticks.SmartJoystick;
 
 public class TrainingRobotManager extends DefaultRobotManager {
 
@@ -19,12 +24,16 @@ public void trainingPeriodic() {
 	@Override
 	public void teleopInit() {
 		// schedule your command...
+		SmartJoystick smartJoystick = new SmartJoystick(JoystickPorts.MAIN);
+
+		smartJoystick.A.whileTrue(new MoveElbowToAngle(Rotation2d.fromDegrees(30), Elbow.getInstance()));
+		smartJoystick.Y.whileTrue(new MoveElbowToAngle(Rotation2d.fromDegrees(50), Elbow.getInstance()));
+		smartJoystick.B.whileTrue(new MoveElbowToAngle(Rotation2d.fromDegrees(80), Elbow.getInstance()));
+		smartJoystick.X.whileTrue(new MoveElbowToAngle(Rotation2d.fromDegrees(100), Elbow.getInstance()));
 	}
 
 
 	@Override
-	public void teleopExit() {
-		// cancel your command...
-	}
+	public void teleopExit() {}
 
 }

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

@@ -0,0 +1,34 @@
+package training.commands.elbow;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import training.subsystems.RobotArm.elbow.Elbow;
+
+public class MoveElbowToAngle extends Command {
+
+	private Rotation2d position;
+	private Elbow elbow;
+
+	public MoveElbowToAngle(Rotation2d position, Elbow elbow) {
+		this.position = position;
+		this.elbow = elbow;
+	}
+
+	@Override
+	public void execute() {
+		elbow.goToAngle(position);
+	}
+
+
+	@Override
+	public boolean isFinished() {
+		return elbow.isAtPosition(position);
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+		elbow.stayInPlace(position);
+	}
+
+}
+

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

@@ -0,0 +1,28 @@
+package training.commands.roller;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import training.subsystems.RobotArm.roller.Roller;
+
+public class MoveRollerAtSpeed extends Command {
+
+	private Roller roller;
+	private Rotation2d velocity;
+
+	public MoveRollerAtSpeed(Rotation2d velocity) {
+		this.velocity = velocity;
+	}
+
+	public void initialize() {
+		Roller.getInstance().moveAtSpeed(velocity.getDegrees());
+	}
+
+	public boolean isFinished() {
+		return (roller.isAtSpeed(velocity));
+	}
+
+	public void end(boolean interrupted) {
+		roller.moveAtSpeed(0);
+	}
+
+}

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

@@ -0,0 +1,32 @@
+package training.commands.wrist;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import training.subsystems.RobotArm.wrist.Wrist;
+
+public class MoveWristToAngle extends Command {
+
+	Wrist wristSubsystem = new Wrist();
+	private Rotation2d wantedAngle;
+
+	public MoveWristToAngle(Rotation2d wantedAngle) {
+		this.wantedAngle = wantedAngle;
+	}
+
+	@Override
+	public void initialize() {}
+
+	@Override
+	public void execute() {
+		wristSubsystem.goToPosition(wantedAngle);
+	}
+
+	@Override
+	public boolean isFinished() {
+		return false;
+	}
+
+	@Override
+	public void end(boolean interrupted) {}
+
+}

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

@@ -0,0 +1,59 @@
+package training.subsystems.RobotArm.elbow;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.Logger;
+import utils.GBSubsystem;
+
+public class Elbow extends GBSubsystem {
+
+	private ElbowInputsAutoLogged inputs;
+	private Rotation2d position;
+	private static Elbow instance;
+	private final IElbow current;
+
+	private Elbow() {
+		this.current = ElbowFactory.create();
+		this.position = new Rotation2d(ElbowConstants.BIGINNING_POSITION);
+		this.inputs = new ElbowInputsAutoLogged();
+	}
+
+	public static Elbow getInstance() {
+		if (instance == null)
+			instance = new Elbow();
+		return instance;
+	}
+
+	public void goToAngle(Rotation2d position) {
+		current.goToAngle(position);
+	}
+
+	public Rotation2d getPosition() {
+		return current.getPosition();
+	}
+
+	public Rotation2d getVelocity() {
+		return current.getVelocity();
+	}
+
+
+	public boolean isAtPosition(Rotation2d position) {
+		return current.isAtPosition(position);
+	}
+
+	public void stayInPlace(Rotation2d position) {
+		current.stayInPlace(position);
+	}
+
+	@Override
+	protected String getLogPath() {
+		return "Elbow/";
+	}
+
+	@Override
+	protected void subsystemPeriodic() {
+		current.updateInputs(inputs);
+		Logger.processInputs(getLogPath(), inputs);
+	}
+
+}
+

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

@@ -0,0 +1,40 @@
+package training.subsystems.RobotArm.elbow;
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public class ElbowConstants {
+
+	public static final double BIGINNING_POSITION = 0, KP = 80, KI = 0, KD = 5, KS = 0, KA = 0, KV = 0, KG = 0,
+		ARE_FEED_FORWARDS = 0;
+	public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(3);
+
+	public enum PresetPositions {
+
+		SAFE(Rotation2d.fromDegrees(0)),
+		INTAKE(Rotation2d.fromDegrees(0)),
+		SCORE(Rotation2d.fromDegrees(0)),
+		STARTING(Rotation2d.fromDegrees(0)),
+		TRANSFER(Rotation2d.fromDegrees(0));
+
+		public final Rotation2d ANGLE;
+
+		PresetPositions(Rotation2d angle) {
+			this.ANGLE = angle;
+		}
+
+	}
+
+	protected static ArmFeedforward ARM_FEED_FORWARD = new ArmFeedforward(KS, KG, KV, KA);
+
+	public static final int MOTOR_ID = 0, PID_SLOT = 0;
+
+	public static final double ARM_LENGTH = 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 ARM_MASS_KG = 0.44;
+
+}

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

@@ -0,0 +1,18 @@
+package training.subsystems.RobotArm.elbow;
+
+import training.GeneralConstants;
+import training.RobotTypes;
+
+public class ElbowFactory {
+
+	public static IElbow create() {
+		if (GeneralConstants.robotTypes == RobotTypes.SIMULATION) {
+			return new ElbowSimulation();
+		}
+
+		else {
+			return new ElbowNEO();
+		}
+	}
+
+}

src/main/java/training/subsystems/RobotArm/elbow/ElbowInputs.java

@@ -0,0 +1,13 @@
+package training.subsystems.RobotArm.elbow;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.AutoLog;
+
+@AutoLog
+public class ElbowInputs {
+
+	public Rotation2d position;
+
+	public Rotation2d velocity;
+
+}

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

@@ -0,0 +1,65 @@
+package training.subsystems.RobotArm.elbow;
+
+import com.revrobotics.CANSparkBase;
+import com.revrobotics.CANSparkLowLevel;
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public class ElbowNEO implements IElbow {
+
+	private final CANSparkMax motor;
+	private Rotation2d position;
+	private static ElbowNEO instance;
+
+	public ElbowNEO() {
+		this.motor = new CANSparkMax(ElbowConstants.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless);
+		this.position = new Rotation2d(ElbowConstants.BIGINNING_POSITION);
+		motor.getPIDController().setP(ElbowConstants.KP);
+		motor.getPIDController().setI(ElbowConstants.KI);
+		motor.getPIDController().setD(ElbowConstants.KD);
+	}
+
+
+	public ElbowNEO getInstance() {
+		if (instance != null)
+			instance = new ElbowNEO();
+		return instance;
+	}
+
+
+	@Override
+	public void goToAngle(Rotation2d position) {
+		motor.getPIDController()
+			.setReference(
+				position.getDegrees(),
+				CANSparkBase.ControlType.kPosition,
+				ElbowConstants.PID_SLOT,
+				ElbowConstants.ARM_FEED_FORWARD.calculate(getPosition().getDegrees(), getVelocity().getDegrees())
+			);
+	}
+
+	public Rotation2d getPosition() {
+		return Rotation2d.fromDegrees(motor.getEncoder().getPosition());
+	}
+
+	public Rotation2d getVelocity() {
+		return Rotation2d.fromDegrees(motor.getEncoder().getVelocity());
+	}
+
+	@Override
+	public boolean isAtPosition(Rotation2d position) {
+		return (Math.abs(getPosition().minus(position).getDegrees()) <= ElbowConstants.TOLERANCE.getDegrees());
+	}
+
+	@Override
+	public void stayInPlace(Rotation2d position) {
+		goToAngle(position);
+	}
+
+	@Override
+	public void updateInputs(ElbowInputsAutoLogged inputs) {
+		inputs.position = getPosition();
+		inputs.velocity = getVelocity();
+	}
+
+}