Merge pull request #21 from Team766/capital-city

Capital city

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

@@ -1,6 +1,7 @@
 package com.team766.robot;
 
 import com.team766.framework.AutonomousMode;
+import com.team766.robot.mechanisms.Intake.GamePieceType;
 import com.team766.robot.procedures.*;
 
 import edu.wpi.first.wpilibj.DriverStation;
@@ -21,9 +22,10 @@ public class AutonomousModes {
 		//    new AutonomousMode("DriveSlow", () -> new DriveStraight(0.4)),
 		// new AutonomousMode("FollowPoints", () -> new FollowPoints()),
 		// new AutonomousMode("ReverseIntake", () -> new ReverseIntake()),
-		// new AutonomousMode("OnePieceExitCommunity", () -> new OnePieceExitCommunity()),
-		new AutonomousMode("OnePieceExitCommunityBalance", () -> new OnePieceExitCommunityBalance()),
-		// AutonomousMode("OnePieceBalance", () -> new OnePieceBalance()),
+		// new AutonomousMode("ScoreHighCube", () -> new ScoreHighCube(GamePieceType.CUBE)),
+		// new AutonomousMode("OnePieceExitCommunity", () -> new OnePieceExitCommunity(GamePieceType.CUBE)),
+		// new AutonomousMode("OnePieceExitCommunityBalance", () -> new OnePieceExitCommunityBalance(GamePieceType.CUBE)),
+		new AutonomousMode("OnePieceBalanceCube", () -> new OnePieceBalance(GamePieceType.CUBE)),
 		// new AutonomousMode("FollowPointsFile", () -> new FollowPoints("FollowPoints.json")),
 		// //new AutonomousMode("FollowPointsH", () -> new FollowPoints(new PointDir[]{new PointDir(0, 0), new PointDir(2, 0), new PointDir(1, 0), new PointDir(1, 1), new PointDir(2, 1), new PointDir(0, 1)})),
 		// new AutonomousMode("DoNothing", () -> new DoNothing()),

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

@@ -111,7 +111,7 @@ public void run(Context context) {
 			if (!isCross && Math.abs(leftJoystickX) + Math.abs(leftJoystickY) + Math.abs(rightJoystickX) > 0) {
 				context.takeOwnership(Robot.drive);
 				// If a button is pressed, drive is just fine adjustment
-				if (leftJoystick.getButton(InputConstants.FINE_DRIVING)) {
+				if (rightJoystick.getButton(InputConstants.FINE_DRIVING)) {
 					Robot.drive.controlFieldOriented(Math.toRadians(Robot.gyro.getGyroYaw()), (leftJoystickX * FINE_DRIVING_COEFFICIENT), (leftJoystickY * FINE_DRIVING_COEFFICIENT), (rightJoystickX * FINE_DRIVING_COEFFICIENT));
 				} else {
           	// On deafault, controls the robot field oriented
@@ -127,27 +127,29 @@ public void run(Context context) {
 			if (boxopGamepad.getPOV() == InputConstants.POV_UP) {
 				new GoForCones().run(context);
 				setLightsForGamePiece();
+				SmartDashboard.putBoolean("Game Piece", true);
 			} else if (boxopGamepad.getPOV() == InputConstants.POV_DOWN) {
 				new GoForCubes().run(context);
 				setLightsForGamePiece();
+				SmartDashboard.putBoolean("Game Piece", false);
 			}
 
 			// look for button presses to queue placement of intake/wrist/elevator superstructure
 			if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_NONE)) {
 				placementPosition = PlacementPosition.NONE;
-				setLightsForPlacement();
+				// setLightsForPlacement();
 			} else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_LOW)) {
 				placementPosition = PlacementPosition.LOW_NODE;
-				setLightsForPlacement();
+				// setLightsForPlacement();
 			} else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_MID)) {
 				placementPosition = PlacementPosition.MID_NODE;
-				setLightsForPlacement();
+				// setLightsForPlacement();
 			} else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_HIGH)) {
 				placementPosition = PlacementPosition.HIGH_NODE;
-				setLightsForPlacement();
+				// setLightsForPlacement();
 			} else if (boxopGamepad.getButton(InputConstants.BUTTON_PLACEMENT_HUMAN_PLAYER)) {
 				placementPosition = PlacementPosition.HUMAN_PLAYER;
-				setLightsForPlacement();
+				// setLightsForPlacement();
 			}
 
 			// look for button hold to start intake, release to idle intake
@@ -175,15 +177,19 @@ public void run(Context context) {
 						context.startAsync(new ExtendWristvatorToHigh());
 						break;
 					case HUMAN_PLAYER:
-						context.startAsync(new ExtendWristvatorToHuman(Robot.intake.getGamePieceType()));
+						context.startAsync(new ExtendToHumanWithIntake(Robot.intake.getGamePieceType()));
 						break;
 					default:
 					// warn, ignore
 					log(Severity.WARNING, "Unexpected placement position: " + placementPosition.toString());
 					break;
 				}
 			} else if (boxopGamepad.getButtonReleased(InputConstants.BUTTON_EXTEND_WRISTVATOR)) {
-				context.startAsync(new RetractWristvator());
+				if (placementPosition == PlacementPosition.HUMAN_PLAYER) {
+					context.startAsync(new RetractWristvatorIdleIntake());
+				} else {
+					context.startAsync(new RetractWristvator());
+				}
 			}
 
 			// look for manual nudges
@@ -226,29 +232,29 @@ public void run(Context context) {
 			}
 
 			if (lightsRateLimit.next()) {
-				if (DriverStation.getMatchTime() > 0 && DriverStation.getMatchTime() < 10) {
+				if (DriverStation.getMatchTime() > 0 && DriverStation.getMatchTime() < 17) {
 					Robot.lights.rainbow();
 				} else {
-					setLightsForPlacement();
+					setLightsForGamePiece();
 				}
 			}
 		}
 	}
 
 	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 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;

src/main/java/com/team766/robot/constants/InputConstants.java

@@ -24,17 +24,17 @@ public final class InputConstants {
 
 	// Boxop Gamepad Buttons
 
-	// LT
-	public static final int BUTTON_INTAKE_IN = 7;
 	// RT
-	public static final int BUTTON_EXTEND_WRISTVATOR = 8;
+	public static final int BUTTON_INTAKE_IN = 8;
+	// LT
+	public static final int BUTTON_EXTEND_WRISTVATOR = 7;
 	// Start
 	public static final int BUTTON_INTAKE_STOP = 10; // used for development
 
-	// left axis
-	public static final int AXIS_WRIST_MOVEMENT = 1;
 	// right axis
-	public static final int AXIS_ELEVATOR_MOVEMENT = 3;
+	public static final int AXIS_WRIST_MOVEMENT = 3;
+	// left axis
+	public static final int AXIS_ELEVATOR_MOVEMENT = 1;
 
 
 	// pov

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

@@ -28,9 +28,9 @@ public enum Position {
 		/** Elevator is at appropriate height to place game pieces at the high node. */
 		HIGH(40), 
 		/** Elevator is at appropriate height to grab cubes from the human player. */
-		HUMAN_CUBES(40),
+		HUMAN_CUBES(40.5),
 		/** Elevator is at appropriate height to grab cones from the human player. */
-		HUMAN_CONES(40),
+		HUMAN_CONES(40.5),
 		/** Elevator is fully extended. */
 		EXTENDED(40);
 

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

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

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

@@ -3,17 +3,18 @@
 import com.ctre.phoenix.led.CANdle;
 import com.ctre.phoenix.led.RainbowAnimation;
 import com.team766.framework.Mechanism;
+import com.team766.robot.constants.SwerveDriveConstants;
 
 
 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);
+	private Animation rainbowAnimation = new RainbowAnimation(1, 1.5, LED_COUNT);
 
 	public Lights(){
-		candle = new CANdle(CANID);
+		candle = new CANdle(CANID, SwerveDriveConstants.SWERVE_CANBUS);
 
 	}
 
@@ -30,7 +31,7 @@ public void white(){
 
 	public void yellow(){
 		checkContextOwnership();
-		candle.setLEDs(255, 255, 0);
+		candle.setLEDs(255, 150, 0);
 	}
 
 	public void red() {

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

@@ -38,7 +38,9 @@ public enum Position {
 		LEVEL(0.0),
 		HIGH_NODE(-30),
 		MID_NODE(-25.5),
-		/** Wrist is fully down. */ 
+		HUMAN_CONES(-17.5),
+		HUMAN_CUBES(-17.5),
+		/** Wrist is fully down. **/
 		BOTTOM(60);
 
 		private final double angle;

src/main/java/com/team766/robot/procedures/OPECHelper.java → src/main/java/com/team766/robot/procedures/ExitCommunity.java

@@ -5,17 +5,13 @@
 import com.team766.robot.Robot;
 import com.team766.robot.constants.FollowPointsInputConstants;
 
-public class OPECHelper extends Procedure {
+public class ExitCommunity extends Procedure {
 
 	private static final double DIST = 4;
 
 	public void run(Context context) {
 		context.takeOwnership(Robot.drive);
-		// context.takeOwnership(Robot.intake);
 		double startY = Robot.drive.getCurrentPosition().getY();
-		// robot gyro is offset 90º from how we want, so we reset it to 90º to account for this
-		Robot.gyro.resetGyro();
-		// new IntakeRelease().run(context);
 		Robot.drive.controlFieldOriented(Math.toRadians(Robot.gyro.getGyroYaw()), 0, -FollowPointsInputConstants.SPEED, 0);
 		context.waitFor(() -> Math.abs(Robot.drive.getCurrentPosition().getY() - startY) > DIST);
 		Robot.drive.stopDrive();

src/main/java/com/team766/robot/procedures/ExtendToHumanWithIntake.java

@@ -0,0 +1,23 @@
+package com.team766.robot.procedures;
+
+import com.team766.framework.Context;
+import com.team766.framework.Procedure;
+import com.team766.robot.Robot;
+import com.team766.robot.mechanisms.Intake.GamePieceType;
+
+public class ExtendToHumanWithIntake extends Procedure {
+	private final GamePieceType gamePieceType;
+	public ExtendToHumanWithIntake(GamePieceType gamePieceType) {
+		this.gamePieceType = gamePieceType;
+	}
+
+	public void run(Context context) {
+		context.takeOwnership(Robot.intake);
+		context.takeOwnership(Robot.wrist);
+		context.takeOwnership(Robot.elevator);
+
+		new IntakeIn().run(context);
+		new ExtendWristvatorToHuman(gamePieceType).run(context);
+	}
+
+}

src/main/java/com/team766/robot/procedures/ExtendWristvatorToHuman.java

@@ -9,7 +9,9 @@ public class ExtendWristvatorToHuman extends MoveWristvator {
 	public ExtendWristvatorToHuman(GamePieceType gamePieceType) {
 		super(gamePieceType == GamePieceType.CONE
 				? Elevator.Position.HUMAN_CONES
-				: Elevator.Position.HUMAN_CUBES,
-			Wrist.Position.LEVEL);
+				: Elevator.Position.HUMAN_CUBES, 
+				gamePieceType == GamePieceType.CONE
+				? Wrist.Position.HUMAN_CONES
+				: Wrist.Position.HUMAN_CUBES);
 	}
 }

src/main/java/com/team766/robot/procedures/GyroBalance.java

@@ -18,7 +18,8 @@ private enum State {
 		GROUND,
 		RAMP_TRANSITION,
 		RAMP_TILT,
-		RAMP_LEVEL
+		RAMP_LEVEL,
+		OVERSHOOT
 	}
 
 	// Direction determines which direction the robot moves
@@ -46,7 +47,9 @@ private enum Direction {
 	private final double CORRECTION_DELAY = 0.7;
 	private final double SPEED_GROUND = .3;
 	private final double SPEED_TRANSITION = .25;
-	private final double SPEED_TILT = .18;
+	private final double SPEED_TILT = .12;
+	private final double SPEED_OVERSHOOT = .08;
+	private final double OVERSHOOT_INCORRECT_MULT = 0.5;
 
 	/** 
 	 * Constructor to create a new GyroBalance instance
@@ -66,6 +69,9 @@ public void run(Context context) {
 		// driveSpeed is actual value of speed passed into the swerveDrive method
 		double driveSpeed = 1;
 
+		// extend wristvator to put CG in a place where robot can climb ramp
+		new ExtendWristvatorToMid().run(context);
+
 		// Sets movement direction ground state if on ground
 		setDir(curY);
 
@@ -85,7 +91,7 @@ public void run(Context context) {
 			tilt = Robot.gyro.getAbsoluteTilt();
 			//log("curX:" + curX);
 			//log("direction: " + direction);
-			setState();
+			setState(context);
 
 			// Both being on Red alliance and needing to move right would make the movement direction negative, so this expression corrects for that
 			if ((alliance == Alliance.Red) ^ (direction == Direction.RIGHT)) {
@@ -100,34 +106,36 @@ public void run(Context context) {
 		}
 		// Loops until robot is level or until a call to the abort() method
 		while (!(curState == State.RAMP_LEVEL));
-
-		// After the robot is level, drives for correctionDelay seconds.
-		// Direction is opposite due to inversion of speed in setState() so it corrects for overshooting
-		context.waitForSeconds(CORRECTION_DELAY);
-
-		// Locks wheels once balanced
-		context.startAsync(new SetCross());
-
-		context.releaseOwnership(Robot.drive);
-		context.releaseOwnership(Robot.gyro);
 	} 
 
 	// Sets state in state machine, see more details in GyroBalance.md
-	private void setState() { 
+	private void setState(Context context) { 
 		if (prevState == State.GROUND && tilt > LEVEL) {
 			curState = State.RAMP_TRANSITION;
 			absSpeed = SPEED_TRANSITION;
 			log("Transition, prevState: " + prevState + ", curState: " + curState);
 		} else if (prevState == State.RAMP_TRANSITION && tilt < TOP_TILT && tilt > FLAP_TILT) {
 			curState = State.RAMP_TILT;
 			absSpeed = SPEED_TILT;
+			context.startAsync(new RetractWristvator());
 			log("Tilt, prevState: " + prevState + ", curState: " + curState);
 		} else if (prevState == State.RAMP_TILT && tilt < LEVEL) {
-			curState = State.RAMP_LEVEL;
+			curState = State.OVERSHOOT;
 			// If level, sets speed to negative to correct for overshooting
+			absSpeed = SPEED_OVERSHOOT;
 			absSpeed = -absSpeed;
+			log("Overshoot, prevState: " + prevState + ", curState: " + curState);
+		}  else if (prevState == State.OVERSHOOT && tilt < LEVEL) {
+			context.startAsync(new SetCross());
 			log("Level, prevState: " + prevState + ", curState: " + curState);
-		} 
+			context.waitForSeconds(1);
+			tilt = Robot.gyro.getAbsoluteTilt();
+			if (tilt < LEVEL) {
+				curState = State.RAMP_LEVEL;
+			} else {
+				absSpeed *= -OVERSHOOT_INCORRECT_MULT;
+			}
+		}
 		if (curState == State.GROUND) {
 			absSpeed = SPEED_GROUND;
 		}

src/main/java/com/team766/robot/procedures/OnePieceBalance.java

@@ -4,15 +4,20 @@
 import com.team766.framework.Procedure;
 import com.team766.odometry.PointDir;
 import com.team766.robot.Robot;
+import com.team766.robot.mechanisms.Intake.GamePieceType;
 
 import edu.wpi.first.wpilibj.DriverStation;
 
 public class OnePieceBalance extends Procedure {
+	private final GamePieceType type;
+
+	public OnePieceBalance(GamePieceType type) {
+		this.type = type;
+	}
 	public void run(Context context) {
 		context.takeOwnership(Robot.drive);
-		//context.takeOwnership(Robot.intake);
 		context.takeOwnership(Robot.gyro);
-		Robot.gyro.resetGyro();
+		Robot.gyro.resetGyro180();
 		switch (DriverStation.getAlliance()) {
 			case Blue:
 				Robot.drive.setCurrentPosition(new PointDir(2.7, 2));
@@ -26,7 +31,7 @@ public void run(Context context) {
 				return;
 
 		}
-		// new IntakeRelease().run(context);
+		new ScoreHigh(type).run(context);
 		new GyroBalance(DriverStation.getAlliance()).run(context);
 	}
 }