Merge pull request #19 from Team766/bringup-mechanisms

Bringup mechanisms

src/main/java/com/team766/library/RateLimiter.java

@@ -10,6 +10,10 @@ public RateLimiter(double periodSeconds) {
 		this.periodSeconds = periodSeconds;
 	}
 
+	public void reset() {
+		nextTime = 0;
+	}
+
 	public boolean next() {
 		final double now = RobotProvider.getTimeProvider().get();
 		if (now > nextTime) {

src/main/java/com/team766/robot/OI.java

@@ -4,14 +4,15 @@
 import com.team766.framework.Context;
 import com.team766.hal.JoystickReader;
 import com.team766.hal.RobotProvider;
+import com.team766.library.RateLimiter;
 import com.team766.logging.Category;
 import com.team766.logging.Severity;
 import com.team766.robot.constants.InputConstants;
 import com.team766.robot.procedures.*;
 import com.team766.simulator.interfaces.ElectricalDevice.Input;
 import com.team766.robot.mechanisms.Drive;
 import com.team766.robot.mechanisms.Intake;
-
+import com.team766.robot.mechanisms.Intake.GamePieceType;
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -33,6 +34,9 @@ public class OI extends Procedure {
 	private static final double FINE_DRIVING_COEFFICIENT = 0.25;
 	double turningValue = 0;
 	boolean manualControl = true;
+	PlacementPosition placementPosition = PlacementPosition.NONE;
+
+	private RateLimiter lightsRateLimit = new RateLimiter(1.3);
 
 	public OI() {
 		loggerCategory = Category.OPERATOR_INTERFACE;
@@ -44,10 +48,9 @@ public OI() {
 
 	public void run(Context context) {
 		context.takeOwnership(Robot.drive);
-		// context.takeOwnership(Robot.intake);
 		context.takeOwnership(Robot.gyro);
+		context.takeOwnership(Robot.lights);
 
-		PlacementPosition placementPosition = PlacementPosition.NONE;
 		boolean elevatorManual = false;
 		boolean wristManual = false;
 
@@ -123,25 +126,28 @@ public void run(Context context) {
 			// first, check if the boxop is making a cone or cube selection
 			if (boxopGamepad.getPOV() == InputConstants.POV_UP) {
 				new GoForCones().run(context);
+				setLightsForGamePiece();
 			} else if (boxopGamepad.getPOV() == InputConstants.POV_DOWN) {
 				new GoForCubes().run(context);
+				setLightsForGamePiece();
 			}
 
 			// look for button presses to queue placement of intake/wrist/elevator superstructure
 			if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_NONE)) {
 				placementPosition = PlacementPosition.NONE;
+				setLightsForPlacement();
 			} else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_LOW)) {
 				placementPosition = PlacementPosition.LOW_NODE;
-				// TODO: update lights
+				setLightsForPlacement();
 			} else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_MID)) {
 				placementPosition = PlacementPosition.MID_NODE;
-				// TODO: update lights
+				setLightsForPlacement();
 			} else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_HIGH)) {
 				placementPosition = PlacementPosition.HIGH_NODE;
-				// TODO: update lights
+				setLightsForPlacement();
 			} else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_HUMAN_PLAYER)) {
 				placementPosition = PlacementPosition.HUMAN_PLAYER;
-				// TODO: update lights
+				setLightsForPlacement();
 			}
 
 			// look for button hold to start intake, release to idle intake
@@ -185,29 +191,85 @@ public void run(Context context) {
 			if (boxopGamepad.getButton(InputConstants.BUTTON_EXTEND_WRISTVATOR)) {
 
 				// look for elevator nudges
-				double elevatorNudgeAxis = boxopGamepad.getAxis(InputConstants.AXIS_ELEVATOR_MOVEMENT);
+				double elevatorNudgeAxis = -1 * boxopGamepad.getAxis(InputConstants.AXIS_ELEVATOR_MOVEMENT);
 				if (Math.abs(elevatorNudgeAxis) > 0.05) {
-					elevatorManual = true;
+					// elevatorManual = true;
 					context.takeOwnership(Robot.elevator);
-					Robot.elevator.nudgeNoPID(elevatorNudgeAxis);
+					// Robot.elevator.nudgeNoPID(elevatorNudgeAxis);
+					if (elevatorNudgeAxis > 0) {
+						Robot.elevator.nudgeUp();
+					} else {
+						Robot.elevator.nudgeDown();
+					}
 					context.releaseOwnership(Robot.elevator);
-				} else if (elevatorManual) {
+				} else if (false && elevatorManual) {
 					Robot.elevator.stopElevator();
 					elevatorManual = false;
 				}
 
 				// look for wrist nudges
-				double wristNudgeAxis = boxopGamepad.getAxis(InputConstants.AXIS_WRIST_MOVEMENT);
+				double wristNudgeAxis = -1 * boxopGamepad.getAxis(InputConstants.AXIS_WRIST_MOVEMENT);
 				if (Math.abs(wristNudgeAxis) > 0.05) {
-					wristManual = true;
+					// wristManual = true;
 					context.takeOwnership(Robot.wrist);
-					Robot.wrist.nudgeNoPID(wristNudgeAxis);
+					// Robot.wrist.nudgeNoPID(wristNudgeAxis);
+					if (wristNudgeAxis > 0) {
+						Robot.wrist.nudgeUp();
+					} else {
+						Robot.wrist.nudgeDown();
+					}
 					context.releaseOwnership(Robot.wrist);
-				} else if (wristManual) {
+				} else if (false && wristManual) {
 					Robot.wrist.stopWrist();
 					wristManual = true;
 				}
 			}
+
+			if (lightsRateLimit.next()) {
+				if (DriverStation.getMatchTime() > 0 && DriverStation.getMatchTime() < 10) {
+					Robot.lights.rainbow();
+				} else {
+					setLightsForPlacement();
+				}
+			}
 		}
 	}
+
+	private void setLightsForPlacement() {
+		switch (placementPosition) {
+			case NONE:
+				Robot.lights.white();
+				break;
+			case LOW_NODE:
+				Robot.lights.green();
+				break;
+			case MID_NODE:
+				Robot.lights.red();
+				break;
+			case HIGH_NODE:
+				Robot.lights.orange();
+				break;
+			case HUMAN_PLAYER:
+				setLightsForGamePiece();
+				break;
+			default:
+				// warn, ignore
+				log(Severity.WARNING, "Unexpected placement position: " + placementPosition.toString());
+				break;
+		}
+
+		lightsRateLimit.reset();
+		lightsRateLimit.next();
+	}
+
+	private void setLightsForGamePiece() {
+		if (Robot.intake.getGamePieceType() == GamePieceType.CUBE) {
+			Robot.lights.purple();
+		} else {
+			Robot.lights.yellow();
+		}
+
+		lightsRateLimit.reset();
+		lightsRateLimit.next();
+	}
 }

src/main/java/com/team766/robot/Robot.java

@@ -9,6 +9,7 @@ public class Robot {
 	public static Elevator elevator;
 	public static Drive drive;
 	public static Gyro gyro;
+	public static Lights lights;
 
 	public static void robotInit() {
 		// Initialize mechanisms here
@@ -17,5 +18,6 @@ public static void robotInit() {
 		elevator = new Elevator();
 		drive = new Drive();
 		gyro = new Gyro();
+		lights = new Lights();
 	}
 }

src/main/java/com/team766/robot/mechanisms/Elevator.java

@@ -22,17 +22,17 @@ public enum Position {
 		/** Elevator is fully retracted. */
 		RETRACTED(0),
 		/** Elevator is the appropriate height to place game pieces at the low node. */
-		LOW(25), 
+		LOW(5), 
 		/** Elevator is the appropriate height to place game pieces at the mid node. */
-		MID(45),
+		MID(18),
 		/** Elevator is at appropriate height to place game pieces at the high node. */
-		HIGH(180), 
+		HIGH(40), 
 		/** Elevator is at appropriate height to grab cubes from the human player. */
-		HUMAN_CUBES(200),
+		HUMAN_CUBES(40),
 		/** Elevator is at appropriate height to grab cones from the human player. */
-		HUMAN_CONES(200),
+		HUMAN_CONES(40),
 		/** Elevator is fully extended. */
-		EXTENDED(250);
+		EXTENDED(40);
 
 		private final int height;
 
@@ -77,7 +77,9 @@ public Elevator() {
 		leftMotor = (CANSparkMax) halLeftMotor;
 		rightMotor = (CANSparkMax) halRightMotor;
 
-		rightMotor.follow(leftMotor);
+		rightMotor.follow(leftMotor, true);
+
+		leftMotor.getEncoder().setPosition(EncoderUtils.elevatorHeightToRotations(Position.RETRACTED.getHeight()));
 
 		pidController = leftMotor.getPIDController();
 		pidController.setFeedbackDevice(leftMotor.getEncoder());
@@ -115,21 +117,21 @@ public void stopElevator() {
 	}
 
 	public void nudgeUp() {
+		System.err.println("Nudging up.");
+
 		double height = getHeight();
 		// NOTE: this could artificially limit nudge range
 		double targetHeight = Math.min(height + NUDGE_INCREMENT, Position.EXTENDED.getHeight());
-		if (targetHeight > height) {
-			moveTo(targetHeight);
-		}
+		System.err.println("Target: " + targetHeight);
+
+		moveTo(targetHeight);
 	}
 
 	public void nudgeDown() {
 		double height = getHeight();
 		// NOTE: this could artificially limit nudge range
 		double targetHeight = Math.max(height - NUDGE_INCREMENT, Position.RETRACTED.getHeight());
-		if (targetHeight < height) {
-			moveTo(targetHeight);
-		}
+		moveTo(targetHeight);
 	}
 
 	/**
@@ -145,26 +147,30 @@ public void moveTo(Position position) {
 	public void moveTo(double position) {
 		checkContextOwnership();
 
+		System.err.println("Setting target position to " + position);
 		// set the PID controller values with whatever the latest is in the config
 		pidController.setP(pGain.get());
 		pidController.setI(iGain.get());
 		pidController.setD(dGain.get());
-		pidController.setFF(ffGain.get());
+		// pidController.setFF(ffGain.get());
+		double ff = ffGain.get();
 
-		pidController.setOutputRange(-1, 1);
 
-		pidController.setSmartMotionAccelStrategy(AccelStrategy.kTrapezoidal, 0);
-		pidController.setSmartMotionMaxVelocity(maxVelocity.get(), 0);
-		pidController.setSmartMotionMinOutputVelocity(minOutputVelocity.get(), 0);
-		pidController.setSmartMotionMaxAccel(maxAccel.get(), 0);
+		pidController.setOutputRange(-0.4, 0.4);
 
-		// TODO: do we need to set output range?
+		// pidController.setSmartMotionAccelStrategy(AccelStrategy.kTrapezoidal, 0);
+		// pidController.setSmartMotionMaxVelocity(maxVelocity.get(), 0);
+		// pidController.setSmartMotionMinOutputVelocity(minOutputVelocity.get(), 0);
+		// pidController.setSmartMotionMaxAccel(maxAccel.get(), 0);
 
 		// convert the desired target degrees to encoder units
 		double rotations = EncoderUtils.elevatorHeightToRotations(position);
 
+		// SmartDashboard.putNumber("[ELEVATOR] ff", ff);
+		SmartDashboard.putNumber("[ELEVATOR] reference", rotations);
+
 		// set the reference point for the wrist
-		pidController.setReference(rotations, ControlType.kSmartMotion);
+		pidController.setReference(rotations, ControlType.kPosition, 0, ff);
 	}
 
 	@Override

src/main/java/com/team766/robot/mechanisms/EncoderUtils.java

@@ -18,7 +18,7 @@ private EncoderUtils() {
 	public static double wristDegreesToRotations(double angle) {
 		// angle * net gear ratio * (rotations / degrees)
 		// FIXME: replace 32 with actual # of teeth
-		return angle * (72. / 32.) * (72. / 20.) * (48. / 24.) * (1. / 360.);
+		return angle * (72. / 10.) * (72. / 20.) * (48. / 24.) * (1. / 360.);
 	}
 
 	/**
@@ -27,15 +27,15 @@ public static double wristDegreesToRotations(double angle) {
 	public static double wristRotationsToDegrees(double rotations) {
 		// rotations * net gear ratio * (degrees / rotations)
 		// FIXME: replace 32 with actual # of teeth
-		return rotations * (32. / 72.) * (20. / 72.) * (24. / 48.) * (360. / 1.);
+		return rotations * (10. / 72.) * (20. / 72.) * (24. / 48.) * (360. / 1.);
 	}
 
 	/**
 	 * Converts a desired height (in inches) to rotations for the elevator motors.
 	 */
 	public static double elevatorHeightToRotations(double height) {
 		// height * net gear ratio * (rotations / height)
-		return height * (8./3.) * (1./(1.641 * Math.PI));
+		return height * (36./12.) * (1./(1.641 * Math.PI));
 	}
 
 	/**
@@ -44,7 +44,7 @@ public static double elevatorHeightToRotations(double height) {
 	public static double elevatorRotationsToHeight(double rotations) {
 		// rotations * net gear ratio * (height / rotations)
 		// FIXME: everything
-		return rotations * (3./8.) * ((1.641 * Math.PI)/1.);
+		return rotations * (12./36.) * ((1.641 * Math.PI)/1.);
 	}
 
 	/**

src/main/java/com/team766/robot/mechanisms/Intake.java

@@ -21,9 +21,9 @@
  */
 public class Intake extends Mechanism {
 
-	private static final double POWER_IN = 1.0;
-	private static final double POWER_OUT = 1.0;
-	private static final double POWER_IDLE = 0.1;
+	private static final double POWER_IN = 0.3;
+	private static final double POWER_OUT = 0.25;
+	private static final double POWER_IDLE = 0.02;
 
 	/**
 	 * The current type of game piece the Intake is preparing to hold or is holding.

src/main/java/com/team766/robot/mechanisms/Lights.java

@@ -1,12 +1,16 @@
 package com.team766.robot.mechanisms;
+import com.ctre.phoenix.led.Animation;
 import com.ctre.phoenix.led.CANdle;
+import com.ctre.phoenix.led.RainbowAnimation;
 import com.team766.framework.Mechanism;
 
 
 public class Lights extends Mechanism{
 
 	private CANdle candle;
 	private static final int CANID = 5;
+	private static final int LED_COUNT = 90;
+	private Animation rainbowAnimation = new RainbowAnimation(1, 0.1, LED_COUNT);
 
 	public Lights(){
 		candle = new CANdle(CANID);
@@ -20,11 +24,32 @@ public void purple(){
 
 	public void white(){
 		checkContextOwnership();
-		candle.setLEDs(255, 255, 255);
+		// NOTE: 255, 255, 255 trips the breaker. lol
+		candle.setLEDs(128, 128, 128);
 	}
 
 	public void yellow(){
 		checkContextOwnership();
 		candle.setLEDs(255, 255, 0);
 	}
+
+	public void red() {
+		checkContextOwnership();
+		candle.setLEDs(255, 0, 0);
+	}
+
+	public void green() {
+		checkContextOwnership();
+		candle.setLEDs(0, 255, 0);
+	}
+
+	public void orange() {
+		checkContextOwnership();
+		candle.setLEDs(255, 64, 0);
+	}
+
+	public void rainbow() {
+		checkContextOwnership();
+		candle.animate(rainbowAnimation);
+	}
 }