Mentor bs code testing3

src/main/java/org/ironriders/core/Robot.java

@@ -4,6 +4,7 @@
 
 package org.ironriders.core;
 
+import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.net.WebServer;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.TimedRobot;
@@ -26,6 +27,7 @@ public class Robot extends TimedRobot {
 	public Robot() {
 		robotContainer = new RobotContainer();
 		WebServer.start(5800, Filesystem.getDeployDirectory().getPath());
+		CameraServer.startAutomaticCapture();
 	}
 
 	@Override

src/main/java/org/ironriders/core/RobotCommands.java

@@ -77,7 +77,7 @@ public Command startup() {
 		algaeIntakeCommands.setOnSuccess(() -> rumble());
 		return coralWristCommands.home()
 				.andThen(algaeWristCommands.home())
-				.andThen(algaeWristCommands.set(AlgaeWristConstants.AlgaeWristState.EXTENDED))
+				.andThen(algaeWristCommands.set(AlgaeWristConstants.AlgaeWristState.STOWED))
 				.andThen(elevatorCommands.home());
 	}
 

src/main/java/org/ironriders/core/RobotContainer.java

@@ -131,6 +131,10 @@ private void configureBindings() {
 		}
 		// y vision align station not implmented yet //TODO
 		// x vision align reef not implmented yet //TODO
+		primaryController.button(1).onTrue(driveCommands.jog(90.0));
+		primaryController.button(2).onTrue(driveCommands.jog(270.0));
+
+
 
 		//Secondary Driver left side buttons
 		secondaryController.button(1).whileTrue(coralIntakeCommands.set(CoralIntakeConstants.CoralIntakeState.EJECT)).whileFalse(coralIntakeCommands.set(CoralIntakeConstants.CoralIntakeState.STOP));
@@ -150,6 +154,9 @@ private void configureBindings() {
 		secondaryController.button(3).onTrue(algaeWristCommands.set(AlgaeWristState.STOWED));
 		secondaryController.button(13).onTrue(climbCommands.goTo(ClimbConstants.Targets.MAX));
 		secondaryController.button(14).onTrue(climbCommands.goTo(ClimbConstants.Targets.TARGET));
+
+		secondaryController.axisGreaterThan(1, .1).whileTrue(algaeIntakeCommands.set(AlgaeIntakeState.EJECT)).whileFalse(algaeIntakeCommands.set(AlgaeIntakeState.STOP));
+		secondaryController.axisLessThan(1, .1).whileTrue(algaeIntakeCommands.set(AlgaeIntakeState.GRAB)).whileFalse(algaeIntakeCommands.set(AlgaeIntakeState.STOP));
 	}
 
 	/**

src/main/java/org/ironriders/drive/DriveConstants.java

@@ -41,6 +41,6 @@ public class DriveConstants {
     public static final double SWERVE_MAXIMUM_ANGULAR_VELOCITY_AUTO = Math.PI * 4; // rad/s
     public static final double SWERVE_MAXIMUM_ANGULAR_ACCELERATION_AUTO = SWERVE_MAXIMUM_ANGULAR_VELOCITY_AUTO / 2; // rad/s^2
 
-    public static final double JOG_DISTANCE_INCHES = 1;
-    public static final double JOG_SPEED = .25;
+    public static final double JOG_DISTANCE_INCHES = 0.5;
+    public static final double JOG_SPEED = .5;
 }

src/main/java/org/ironriders/drive/Vision.java

@@ -46,9 +46,9 @@ public Vision(SwerveDrive drive) {
         cams.add(new VisionCamera("frontLeft",
                 createOffset(11.5, 11.5, 6.5, 15, 22.5),
                 VecBuilder.fill(0.5, 0.5, 1.0)));
-        cams.add(new VisionCamera("backLeft",
-                createOffset(11.5, -11.5, 6.5, 15, 157.5),
-                VecBuilder.fill(0.5, 0.5, 1.0)));
+       // cams.add(new VisionCamera("backLeft",
+        //        createOffset(11.5, -11.5, 6.5, 15, 157.5),
+         //       VecBuilder.fill(0.5, 0.5, 1.0)));
         // cams.add(new VisionCamera("backRight",
         // createOffset(-11.5, 11.5, 6.5, 15, -157.5),
         // VecBuilder.fill(0.5, 0.5, 1.0)));

src/main/java/org/ironriders/wrist/algae/AlgaeWristCommands.java

@@ -17,18 +17,18 @@ public AlgaeWristCommands(AlgaeWristSubsystem wrist) {
     }
 
     public Command home() {
-        //return algaeWrist.homeCmd();
-        return Commands.none();
+        return algaeWrist.homeCmd();
+        //return Commands.none();
     }
 
     public Command set(AlgaeWristState state) {
-        //return algaeWrist.moveToCmd(state.getAngle());
-        return Commands.none();
+        return algaeWrist.moveToCmd(state.getAngle());
+        //return Commands.none();
     }
 
     public Command reset() {
-        //return algaeWrist.runOnce(algaeWrist::reset);
-        return Commands.none();
+        return algaeWrist.runOnce(algaeWrist::reset);
+        //return Commands.none();
     }
 
     public AlgaeWristSubsystem getAlgaeWrist() {

src/main/java/org/ironriders/wrist/algae/AlgaeWristConstants.java

@@ -17,7 +17,7 @@ public class AlgaeWristConstants {
     public static final double GEAR_RATIO = 1.0 / 64.0 * SPROCKET_RATIO;
     public static final double ENCODER_SCALE = SPROCKET_RATIO;
 
-    public static final Angle ENCODER_OFFSET = Units.Degrees.of(115);
+    public static final Angle ENCODER_OFFSET = Units.Degrees.of(150);
     public static final Angle REVERSE_LIMIT = Units.Degrees.of(-50);
     public static final Angle FORWARD_LIMIT = Units.Degrees.of(5);
 
@@ -28,9 +28,9 @@ public class AlgaeWristConstants {
     public static final double ALGAE_WRIST_TOLERANCE = 2; // tune me please
 
     public enum AlgaeWristState {
-        STARTING(-90), // Starting position is beyond lower limit, exact position unknownn
+        STARTING(-60), // Starting position is beyond lower limit, exact position unknownn
         STOWED(-40), // Computed home position
-        EXTENDED(10);
+        EXTENDED(0);
 
         final Angle angle;