Maya arm

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

@@ -0,0 +1,32 @@
+package training.commands.elbow;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import training.subsystems.elbow.Elbow;
+
+public class MoveElbowToAngle extends Command {
+
+	private Rotation2d targetAngle;
+
+	public Elbow getInstance() {
+		return Elbow.getInstance();
+	}
+
+	public MoveElbowToAngle(Rotation2d targetAngle) {
+		this.targetAngle = targetAngle;
+	}
+
+	@Override
+	public void initialize() {
+		getInstance().goToAngle(targetAngle);
+	}
+
+	public boolean isFinished() {
+		return getInstance().isAtAngle(targetAngle);
+	}
+
+	public void end(boolean interrupted) {
+		getInstance().stayAtPosition();
+	}
+
+}

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

@@ -0,0 +1,26 @@
+package training.commands.roller;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import training.subsystems.roller.Roller;
+
+public class MoveToSpeedRoller extends Command {
+
+	private double targetSpeed;
+
+	public Roller getInstance() {
+		return Roller.getInstance();
+	}
+
+	public void MoveToSpeedSpeed(double targetSpeed) {
+		this.targetSpeed = targetSpeed;
+	}
+
+	public boolean isFinished() {
+		return getInstance().isAtVelocity(targetSpeed);
+	}
+
+	public void end(boolean interrupted) {
+		getInstance().stop();
+	}
+
+}

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.wrist.Wrist;
+
+public class MoveWristToAngle extends Command {
+
+	private Rotation2d targetAngle;
+
+	public Wrist getInstance() {
+		return Wrist.getInstance();
+	}
+
+	public MoveWristToAngle(Rotation2d targetAngle) {
+		this.targetAngle = targetAngle;
+	}
+
+	@Override
+	public void initialize() {
+		getInstance().goToAngle(targetAngle);
+	}
+
+	public boolean isFinished() {
+		return getInstance().isAtAngle(targetAngle);
+	}
+
+	public void end(boolean interrupted) {
+		getInstance().stop();
+	}
+
+}

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

@@ -0,0 +1,70 @@
+package training.subsystems.elbow;
+
+import com.revrobotics.CANSparkBase;
+import com.revrobotics.CANSparkLowLevel;
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.geometry.Rotation2d;
+import utils.GBSubsystem;
+
+public class Elbow extends GBSubsystem {
+
+	private static Elbow instance;
+	private final CANSparkMax motor;
+	private final ArmFeedforward armFeedforward;
+
+	private Elbow() {
+		this.armFeedforward = ElbowConstants.ARM_FEEDFORWARD;
+		this.motor = new CANSparkMax(ElbowConstants.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless);
+		motor.getPIDController().setP(ElbowConstants.P);
+		motor.getPIDController().setI(ElbowConstants.I);
+		motor.getPIDController().setD(ElbowConstants.D);
+	}
+
+	public static void init() {
+		if (instance == null) {
+			instance = new Elbow();
+		}
+	}
+
+	public static Elbow getInstance() {
+		init();
+		return instance;
+	}
+
+	public Rotation2d getPosition() {
+		return Rotation2d.fromRotations(motor.getEncoder().getPosition());
+	}
+
+	public Rotation2d getVelocity() {
+		return Rotation2d.fromRotations(motor.getEncoder().getVelocity());
+	}
+
+	public void goToAngle(Rotation2d targetAngle) {
+		motor.getPIDController()
+			.setReference(
+				targetAngle.getDegrees(),
+				CANSparkBase.ControlType.kPosition,
+				ElbowConstants.PID_SLOT,
+				ElbowConstants.ARM_FEEDFORWARD.calculate(getPosition().getRadians(), getVelocity().getRotations())
+			);
+	}
+
+	public boolean isAtAngle(Rotation2d targetAngle) {
+		return Math.abs(targetAngle.getDegrees() - motor.getEncoder().getPosition()) <= ElbowConstants.TOLERANCE.getRadians();
+	}
+
+	public void stayAtPosition() {
+		goToAngle(getPosition());
+	}
+
+	@Override
+	protected String getLogPath() {
+		return "Elbow/";
+	}
+
+	@Override
+	protected void subsystemPeriodic() {
+	}
+
+}

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

@@ -0,0 +1,44 @@
+package training.subsystems.elbow;
+
+
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public class ElbowConstants {
+
+	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;
+		}
+
+	}
+
+
+	public static final int MOTOR_ID = 5;
+	public static final int P = 1;
+	public static final int I = 0;
+	public static final int D = 0;
+
+	public static final int PID_SLOT = 0;
+
+	public static final int KS = 0;
+	public static final int KA = 0;
+	public static final int KG = 0;
+	public static final int KV = 0;
+	public static final ArmFeedforward ARM_FEEDFORWARD = new ArmFeedforward(0, 0, 0, 0);
+	public static final double GEAR_RATIO = 1 / (28.0 * (60.0 / 16.0));
+	public static final double ARM_LENGTH = 0.44;
+	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 Rotation2d TOLERANCE = Rotation2d.fromDegrees(3);
+}

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

@@ -0,0 +1,4 @@
+package training.subsystems.elbow;
+
+public class ElbowFactory {
+}

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

@@ -0,0 +1,61 @@
+package training.subsystems.elbow;
+
+import com.revrobotics.CANSparkBase;
+import com.revrobotics.CANSparkLowLevel;
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public class ElbowNEO implements IElbow{
+
+	private CANSparkMax motor;
+	private static ElbowNEO instance;
+	private ArmFeedforward armFeedforward;
+
+	private ElbowNEO() {
+		this.armFeedforward = ElbowConstants.ARM_FEEDFORWARD;
+		this.motor = new CANSparkMax(ElbowConstants.MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless);
+		motor.getPIDController().setP(ElbowConstants.P);
+		motor.getPIDController().setI(ElbowConstants.I);
+		motor.getPIDController().setD(ElbowConstants.D);
+	}
+
+	public void init() {
+		if (instance == null) {
+			instance = new ElbowNEO();
+		}
+	}
+
+	public ElbowNEO getInstance() {
+		init();
+		return instance;
+	}
+
+	public Rotation2d getPosition() {
+		return Rotation2d.fromRotations(motor.getEncoder().getPosition());
+	}
+
+	public Rotation2d getVelocity() {
+		return Rotation2d.fromRotations(motor.getEncoder().getVelocity());
+	}
+
+
+	public void goToAngle(Rotation2d targetAngle) {
+		motor.getPIDController()
+			.setReference(
+				targetAngle.getDegrees(),
+				CANSparkBase.ControlType.kPosition,
+				ElbowConstants.PID_SLOT,
+				ElbowConstants.ARM_FEEDFORWARD.calculate(getPosition().getRadians(), getVelocity().getRotations())
+			);
+	}
+
+	public boolean isAtAngle(Rotation2d targetAngle) {
+		return Math.abs(targetAngle.getDegrees() - motor.getEncoder().getPosition()) == 0;
+	}
+
+	public void stayAtPosition() {
+		goToAngle(getPosition());
+	}
+
+}

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

@@ -0,0 +1,55 @@
+package training.subsystems.elbow;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import edu.wpi.first.math.controller.ArmFeedforward;
+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 utils.simulation.SingleJointedArmSimulation;
+
+public class ElbowSimulation implements IElbow {
+
+	private SingleJointedArmSimulation elbowSimulation;
+	private ArmFeedforward armFeedforward;
+	private PIDController controller;
+	private PositionVoltage positionVoltage;
+
+	private ElbowSimulation() {
+		SingleJointedArmSim armSim = new SingleJointedArmSim(
+			DCMotor.getFalcon500(SimulationElbowConstants.NUMBER_OF_MOTORS),
+			SimulationElbowConstants.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()
+		);
+		elbowSimulation = new SingleJointedArmSimulation(armSim);
+		controller = new PIDController(SimulationElbowConstants.P, SimulationElbowConstants.I, SimulationElbowConstants.D);
+		TalonFXConfiguration config = new TalonFXConfiguration();
+		config.Slot0.kP = controller.getP();
+		config.Slot0.kI = controller.getI();
+		config.Slot0.kD = controller.getD();
+		elbowSimulation.applyConfiguration(config);
+	}
+
+	public void goToAngle(Rotation2d targetAngle) {
+		elbowSimulation.setControl(positionVoltage.withPosition(targetAngle.getRotations()));
+	}
+
+	public Rotation2d getPosition() {
+		return elbowSimulation.getPosition();
+	}
+
+	public Rotation2d getVelocity() {
+		return elbowSimulation.getVelocity();
+	}
+
+	public void stayAtPosition() {
+		goToAngle(getPosition());
+	}
+
+}

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

@@ -0,0 +1,14 @@
+package training.subsystems.elbow;
+
+public class SimulationElbowConstants {
+
+	public static final int NUMBER_OF_MOTORS = 1;
+	public static final double GEAR_RATIO = 1 / ElbowConstants.GEAR_RATIO;
+
+	public static final double P = 1;
+
+	public static final double I = 0;
+
+	public static final double D = 0;
+
+}

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

@@ -0,0 +1,7 @@
+package training.subsystems.roller;
+
+public interface IRoller {
+
+	void goToSpeed(double targetSpeed);
+
+}