Maya arm

networktables.json

@@ -0,0 +1 @@
+[]

simgui-ds.json

@@ -0,0 +1,97 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ],
+  "robotJoysticks": [
+    {
+      "guid": "Keyboard0"
+    }
+  ]
+}

simgui.json

@@ -0,0 +1,10 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo"
+    }
+  },
+  "NetworkTables Info": {
+    "visible": true
+  }
+}

src/main/java/training/Robot.java

@@ -1,11 +1,14 @@
 package training;
 
+import training.subsystems.elbow.Elbow;
+
 public class Robot {
 
 	// Enter your subsystems...
 
 	public Robot() {
 		// Boot your subsystems...
+		Elbow.init();
 	}
 
 	// Add your subsystems getters...

src/main/java/training/TrainingRobotManager.java

@@ -1,14 +1,28 @@
 package training;
 
+import edu.wpi.first.math.geometry.Rotation2d;
+import training.commands.elbow.MoveElbowToAngle;
+import training.subsystems.elbow.RobotTypes;
 import utils.DefaultRobotManager;
+import utils.joysticks.JoystickPorts;
+import utils.joysticks.SmartJoystick;
 
 public class TrainingRobotManager extends DefaultRobotManager {
 
 	private Robot robot;
 
+	public static final RobotTypes ROBOT_TYPE = RobotTypes.SIMULATION;
+
 	@Override
 	public void trainingInit() {
 		this.robot = new Robot();
+		configureBindings();
+	}
+
+	public void configureBindings() {
+		SmartJoystick smartJoystick = new SmartJoystick(JoystickPorts.MAIN);
+		smartJoystick.A.whileTrue(new MoveElbowToAngle(Rotation2d.fromDegrees(45)));
+		smartJoystick.B.whileTrue(new MoveElbowToAngle(Rotation2d.fromDegrees(90)));
 	}
 
 	@Override

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

@@ -0,0 +1,31 @@
+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;
+	private Elbow elbow;
+
+	public MoveElbowToAngle(Rotation2d targetAngle) {
+		this.targetAngle = targetAngle;
+		this.elbow = Elbow.getInstance();
+		addRequirements(this.elbow);
+	}
+
+	@Override
+	public void execute() {
+		elbow.goToAngle(targetAngle);
+	}
+
+	public boolean isFinished() {
+		return elbow.isAtAngle(targetAngle);
+	}
+
+	public void end(boolean interrupted) {
+		elbow.stayAtPosition();
+	}
+
+}

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

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

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

@@ -0,0 +1,31 @@
+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;
+	private Wrist wrist;
+
+	public MoveWristToAngle(Rotation2d targetAngle) {
+		this.targetAngle = targetAngle;
+		this.wrist = Wrist.getInstance();
+		addRequirements(wrist);
+	}
+
+	@Override
+	public void execute() {
+		wrist.goToAngle(targetAngle);
+	}
+
+	public boolean isFinished() {
+		return wrist.isAtAngle(targetAngle);
+	}
+
+	public void end(boolean interrupted) {
+		wrist.stayAtPosition();
+	}
+
+}

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

@@ -0,0 +1,62 @@
+package training.subsystems.elbow;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.Logger;
+import utils.GBSubsystem;
+
+public class Elbow extends GBSubsystem {
+
+	private static Elbow instance;
+
+	private final IElbow elbow;
+
+	private ElbowInputsAutoLogged inputs;
+
+	private Elbow() {
+		this.elbow = ElbowFactory.create();
+		this.inputs = new ElbowInputsAutoLogged();
+	}
+
+	public static void init() {
+		if (instance == null) {
+			instance = new Elbow();
+		}
+	}
+
+	public static Elbow getInstance() {
+		init();
+		return instance;
+	}
+
+	public Rotation2d getPosition() {
+		return inputs.position;
+	}
+
+	public Rotation2d getVelocity() {
+		return inputs.velocity;
+	}
+
+	public void goToAngle(Rotation2d targetAngle) {
+		elbow.goToAngle(targetAngle);
+	}
+
+	public boolean isAtAngle(Rotation2d targetAngle) {
+		return Math.abs(targetAngle.getRotations() - inputs.position.getRotations()) <= ElbowConstants.TOLERANCE.getRotations();
+	}
+
+	public void stayAtPosition() {
+		goToAngle(getPosition());
+	}
+
+	@Override
+	protected String getLogPath() {
+		return "Elbow/";
+	}
+
+	@Override
+	protected void subsystemPeriodic() {
+		elbow.updateInputs(inputs);
+		Logger.processInputs("Elbow/", inputs);
+	}
+
+}

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

@@ -0,0 +1,48 @@
+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 = 10;
+
+	public static final int I = 0;
+
+	public static final int D = 1;
+
+	public static final int PID_SLOT = 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,14 @@
+package training.subsystems.elbow;
+
+import training.TrainingRobotManager;
+
+public class ElbowFactory {
+
+	public static IElbow create() {
+		return switch (TrainingRobotManager.ROBOT_TYPE) {
+			case SIMULATION -> new ElbowSimulation();
+			case SYNCOPA -> new NeoElbow();
+		};
+	}
+
+}

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

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