Tomer-arm

src/main/java/training/GeneralConstants.java

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

src/main/java/training/RobotType.java

@@ -0,0 +1,8 @@
+package training;
+
+public enum RobotType {
+
+	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,6 +24,12 @@ 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()));
 	}
 
 

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,35 @@
+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 {
+
+	private Wrist wristSubsystem;
+	private Rotation2d wantedAngle;
+
+	public MoveWristToAngle(Rotation2d wantedAngle, Wrist wristSubsystem) {
+		this.wantedAngle = wantedAngle;
+		this.wristSubsystem = wristSubsystem;
+	}
+
+	@Override
+	public void initialize() {}
+
+	@Override
+	public void execute() {
+		wristSubsystem.goToPosition(wantedAngle);
+	}
+
+	@Override
+	public boolean isFinished() {
+		return wristSubsystem.isAtPosition(wantedAngle);
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+		wristSubsystem.stayInPlace();
+	}
+
+}

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

@@ -0,0 +1,58 @@
+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 iElbow;
+
+	private Elbow() {
+		this.iElbow = ElbowFactory.create();
+		this.position = new Rotation2d(ElbowConstants.BEGINNING_POSITION);
+		this.inputs = new ElbowInputsAutoLogged();
+	}
+
+	public static Elbow getInstance() {
+		if (instance == null)
+			instance = new Elbow();
+		return instance;
+	}
+
+	public void goToAngle(Rotation2d position) {
+		iElbow.goToAngle(position);
+	}
+
+	public Rotation2d getPosition() {
+		return inputs.position;
+	}
+
+	public Rotation2d getVelocity() {
+		return inputs.velocity;
+	}
+
+	public boolean isAtPosition(Rotation2d position) {
+		return Math.abs(getPosition().minus(position).getDegrees()) <= ElbowConstants.TOLERANCE.getDegrees();
+	}
+
+	public void stayInPlace(Rotation2d position) {
+		iElbow.stayInPlace();
+	}
+
+	@Override
+	protected String getLogPath() {
+		return "Elbow/";
+	}
+
+	@Override
+	protected void subsystemPeriodic() {
+		iElbow.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 KP = 30, KI = 0, KD = 15, KS = 0, KA = 0, KV = 0, KG = 0, ARE_FEED_FORWARDS = 0;
+	public static final Rotation2d TOLERANCE = Rotation2d.fromDegrees(3);
+	public static final double BEGINNING_POSITION = 0;
+
+	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,14 @@
+package training.subsystems.RobotArm.elbow;
+
+import training.GeneralConstants;
+
+public class ElbowFactory {
+
+	public static IElbow create() {
+		return switch (GeneralConstants.ROBOT_TYPE) {
+			case SYNCOPA -> new ElbowNEO();
+			case SIMULATION -> new ElbowSimulation();
+		};
+	}
+
+}

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,50 @@
+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;
+
+	public ElbowNEO() {
+		this.motor = new CANSparkMax(ElbowConstants.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless);
+		this.position = new Rotation2d(ElbowConstants.BEGINNING_POSITION);
+		motor.getPIDController().setP(ElbowConstants.KP);
+		motor.getPIDController().setI(ElbowConstants.KI);
+		motor.getPIDController().setD(ElbowConstants.KD);
+	}
+
+	@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 void stayInPlace() {
+		goToAngle(position);
+	}
+
+	@Override
+	public void updateInputs(ElbowInputsAutoLogged inputs) {
+		inputs.position = getPosition();
+	}
+
+}

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

@@ -0,0 +1,63 @@
+package training.subsystems.RobotArm.elbow;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import org.littletonrobotics.junction.Logger;
+import utils.simulation.SingleJointedArmSimulation;
+
+public class ElbowSimulation implements IElbow {
+
+	private SingleJointedArmSimulation motor;
+	private Rotation2d position;
+	private PIDController controller;
+
+	public ElbowSimulation() {
+		SingleJointedArmSim armSim = new SingleJointedArmSim(
+			DCMotor.getFalcon500(ElbowSimulationConstants.NUMBER_OF_MOTORS),
+			ElbowSimulationConstants.GEAR_RATIO,
+			SingleJointedArmSim.estimateMOI(ElbowConstants.ARM_LENGTH, ElbowConstants.ARM_MASS_KG),
+			ElbowConstants.ARM_LENGTH,
+			ElbowConstants.BACKWARD_ANGLE_LIMIT.getRadians(),
+			ElbowConstants.FORWARD_ANGLE_LIMIT.getRadians(),
+			false,
+			ElbowConstants.PresetPositions.STARTING.ANGLE.getRadians()
+		) {
+
+		};
+		this.motor = new SingleJointedArmSimulation(armSim);
+		TalonFXConfiguration config = new TalonFXConfiguration();
+		config.Slot0.kP = ElbowConstants.KP;
+		config.Slot0.kI = ElbowConstants.KI;
+		config.Slot0.kD = ElbowConstants.KD;
+		motor.applyConfiguration(config);
+	}
+
+	public Rotation2d getPosition() {
+		return motor.getPosition();
+	}
+
+	public void goToAngle(Rotation2d targetAngle) {
+		PositionVoltage pos = new PositionVoltage(targetAngle.getRotations());
+		motor.setControl(pos);
+		Logger.recordOutput("elbow pos dif", targetAngle.minus(getPosition()));
+	}
+
+	public void stop() {
+		goToAngle(getPosition());
+	}
+
+	@Override
+	public void stayInPlace() {
+		goToAngle(getPosition());
+	}
+
+	@Override
+	public void updateInputs(ElbowInputsAutoLogged inputs) {
+		inputs.position = getPosition();
+	}
+
+}

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

@@ -0,0 +1,10 @@
+package training.subsystems.RobotArm.elbow;
+
+public class ElbowSimulationConstants {
+
+	public static final int NUMBER_OF_MOTORS = 1;
+
+	public static final double GEAR_RATIO = 1;
+	final static double P = 5, I = 0, D = 0;
+
+}