Bridgewater

src/org/robockets/robot/OI.java

@@ -12,6 +12,7 @@
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
 import edu.wpi.first.wpilibj.command.Command;
 import org.robockets.commons.RelativeDirection;
+import org.robockets.robot.climber.Climb;
 import org.robockets.robot.cubeintake.IntakeCube;
 import org.robockets.robot.cubeintake.IntakeOut;
 import org.robockets.robot.drivetrain.FakeDriveAssisted;
@@ -31,6 +32,7 @@ public class OI {
 
 	public static Button driverA = new JoystickButton(driverJoystick, 1);
 	public static Button driverB = new JoystickButton(driverJoystick, 2);
+	public static Button driverY = new JoystickButton(driverJoystick, 4);
 	public static Button driverSelect = new JoystickButton(driverJoystick, 7);
 	public static Button driverStart = new JoystickButton(driverJoystick, 8);
 
@@ -43,17 +45,18 @@ public class OI {
 	public static Button operatorRightStick = new JoystickButton(operatorJoystick, 10);
 
 	public OI() {
-		driverA.whileHeld(new IntakeCube(0.75, 0.4, 0.5));
+		driverA.whileHeld(new IntakeCube(0.9, 0.6, 0.8));
 		driverB.whileHeld(new IntakeOut(0.75, 0.4));
+		driverY.whileHeld(new Climb(0.75));
 		driverSelect.whenPressed(new Joyride());
 		driverStart.whenPressed(new FakeDriveAssisted());
 
-		operatorA.whileHeld(new IntakeCube(0.75, 0.4, 0.65));
-		operatorB.whenPressed(new Elevate(ElevatorPosition.BOTTOM, 0.5));
-		operatorX.whenPressed(new Elevate(ElevatorPosition.EXCHANGE, 0.5));
-		operatorY.whenPressed(new Elevate(ElevatorPosition.SWITCH, 0.5));
+		operatorA.whileHeld(new IntakeCube(0.9, 0.6, 0.8));
+		//operatorB.whenPressed(new Elevate(ElevatorPosition.BOTTOM, 0.5));
+		//operatorX.whenPressed(new Elevate(ElevatorPosition.EXCHANGE, 0.5));
+		//operatorY.whenPressed(new Elevate(ElevatorPosition.SWITCH, 0.5));
 		operatorSelect.whenPressed(new InterruptElevator());
-		operatorLeftStick.whenPressed(new Elevate(ElevatorPosition.MID_SCALE, 0.75));
+		//operatorLeftStick.whenPressed(new Elevate(ElevatorPosition.MID_SCALE, 0.75));
 		//operatorRightStick.whenPressed(new Elevate(ElevatorPosition.HIGH_SCALE, 0.75));
 	}
 }

src/org/robockets/robot/Robot.java

@@ -20,6 +20,7 @@
 import org.robockets.robot.drivetrain.ResetEncoders;
 import org.robockets.robot.drivetrain.StartGyroPID;
 import org.robockets.robot.elevator.Elevator;
+import org.robockets.robot.elevator.ElevatorCounter;
 import org.robockets.robot.elevator.ElevatorFloor;
 import org.robockets.robot.climber.Climber;
 import org.robockets.robot.autonomous.AutoChooser;
@@ -51,6 +52,7 @@ public class Robot extends TimedRobot {
 	public static Command intakeListener;
 	public static Command elevatorFloorListener;
 	public static Command manualElevate;*/
+	public static Command elevatorCounter;
 
 	public static Elevator elevator;
 
@@ -73,17 +75,21 @@ public void robotInit() {
 		RobotMap.gyro.reset();
 		RobotMap.elevatorMotor.getSensorCollection().setPulseWidthPosition(0, 0);
 
+		RobotMap.leftEncoder.setDistancePerPulse(1.0 / 39.92);
+		RobotMap.rightEncoder.setDistancePerPulse(1.0 / 39.92);
+
 		drivetrain = new Drivetrain();
 
 		cubeIntake = new CubeIntake();
 		elevator = new Elevator();
 		elevatorFloor = new ElevatorFloor();
+		elevatorCounter = new ElevatorCounter();
+
 		RobotMap.elevatorMotor.setInverted(true);
+		RobotMap.elevatorFloorMotor.setInverted(true);
 		RobotMap.elevatorFloorMotor2.setInverted(true);
-
-		RobotMap.leftEncoder.setDistancePerPulse(1 / 39.92);
-		RobotMap.rightEncoder.setDistancePerPulse(1 / 39.92);
 		RobotMap.rightEncoder.setReverseDirection(true);
+
 		RobotMap.leftDrivepodSpeedController.setInverted(true);
 		RobotMap.rightDrivepodSpeedController.setInverted(true);
 
@@ -93,15 +99,10 @@ public void robotInit() {
 		SmartDashboard.putNumber("Gyro Setpoint", 0);
 		SmartDashboard.putData(new StartGyroPID());*/
 
-		SmartDashboard.putNumber("Left Encoder P", drivetrain.leftPodPID.getP());
-		SmartDashboard.putNumber("Left Encoder I", drivetrain.leftPodPID.getI());
-		SmartDashboard.putNumber("Left Encoder D", drivetrain.leftPodPID.getD());
-		SmartDashboard.putNumber("Left Encoder Setpoint", 0);
-
-		SmartDashboard.putNumber("Right Encoder P", drivetrain.rightPodPID.getP());
-		SmartDashboard.putNumber("Right Encoder I", drivetrain.rightPodPID.getI());
-		SmartDashboard.putNumber("Right Encoder D", drivetrain.rightPodPID.getD());
-		SmartDashboard.putNumber("Right Encoder Setpoint", 0);
+		SmartDashboard.putNumber("Encoder P", drivetrain.encoderPID.getP());
+		SmartDashboard.putNumber("Encoder I", drivetrain.encoderPID.getI());
+		SmartDashboard.putNumber("Encoder D", drivetrain.encoderPID.getD());
+		SmartDashboard.putNumber("Encoder Setpoint", 0);
 		SmartDashboard.putData(new StartEncoderPID());
 
 		autoChooser.addObject("Test Auto", AutoHelper.AutoType.TEST);
@@ -128,6 +129,8 @@ public void robotInit() {
 
 		SmartDashboard.putData("Reset Encoders", new ResetEncoders());
 
+		elevatorCounter.start();
+
 		m_oi = new OI();
 	}
 
@@ -141,8 +144,7 @@ public void robotPeriodic() {
 		SmartDashboard.putNumber("Left Distance", RobotMap.leftEncoder.getDistance());
 		SmartDashboard.putNumber("Right Distance", RobotMap.rightEncoder.getDistance());
 
-		SmartDashboard.putNumber("Left PID Output", drivetrain.leftPodPID.get());
-		SmartDashboard.putNumber("Right PID Output", drivetrain.rightPodPID.get());
+		SmartDashboard.putNumber("Encoder PID Output", drivetrain.encoderPID.get());
 		/*SmartDashboard.putNumber("PDP0: ", RobotMap.pdp.getCurrent(0));
 		SmartDashboard.putNumber("PDP1: ", RobotMap.pdp.getCurrent(1));
 		SmartDashboard.putNumber("PDP14: ", RobotMap.pdp.getCurrent(14));

src/org/robockets/robot/RobotMap.java

@@ -35,20 +35,17 @@ public class RobotMap {
 	public static Victor leftIntake = new Victor(4); // STROUT!!!!
 	public static Victor barIntake = new Victor(5);
 
-	public static Victor climberMotorLeft = new Victor(7);
-	public static Victor climberMotorRight = new Victor(3);
+	public static Victor climberMotor = new Victor(7);
 
 	public static DifferentialDrive robotDrive = new DifferentialDrive(leftDrivepodSpeedController, rightDrivepodSpeedController);
 
 	public static ADXRS450_Gyro gyro = new ADXRS450_Gyro(SPI.Port.kOnboardCS0);
 
-	public static Encoder leftEncoder = new Encoder(0, 1);
+	public static Encoder leftEncoder = new Encoder(8, 9);
 	public static Encoder rightEncoder = new Encoder(2, 3);
 
 	public static PowerDistributionPanel pdp = new PowerDistributionPanel(0); // Zero is the CAN id of the powerdistribution panel.
 	public static int climberMotorPDPChannel = 1; // Dummy value.
 
-	public static DigitalInput elevatorEncoder = new DigitalInput(5);
-
-	public static Counter counter = new Counter(elevatorEncoder);
+	public static DigitalInput elevatorEncoder = new DigitalInput(4);
 }

src/org/robockets/robot/autonomous/AutoChooser.java

@@ -12,7 +12,7 @@ public AutoChooser(AutoHelper.AutoType auto, AutoHelper.RobotPosition startingPo
 		Command autoCommand = new DumbAuto();
 		switch (auto) {
 			case TEST:
-				autoCommand = new TestAuto2();
+				autoCommand = new TestAuto();
 				break;
 			case DUMB:
 				autoCommand = new DumbAuto();

src/org/robockets/robot/autonomous/DumbAuto.java

@@ -9,6 +9,6 @@
 public class DumbAuto extends CommandGroup {
 
 	public DumbAuto() {
-		addSequential(new DriveStraight(15, .5)); // TODO: Change this to a correct number
+		addSequential(new DriveStraight(10, .5)); // TODO: Change this to a correct number
 	}
 }

src/org/robockets/robot/autonomous/MidAuto.java

@@ -5,12 +5,14 @@
 import edu.wpi.first.wpilibj.command.WaitCommand;
 import org.robockets.robot.Robot;
 import org.robockets.robot.cubeintake.DropCube;
+import org.robockets.robot.cubeintake.IntakeCube;
 import org.robockets.robot.drivetrain.DriveAngleAssisted;
 import org.robockets.robot.drivetrain.DriveStraight;
 import org.robockets.robot.drivetrain.DriveStraightAssisted;
 import org.robockets.robot.drivetrain.TurnAbsolute;
 import org.robockets.robot.elevator.Elevate;
 import org.robockets.robot.elevator.ElevatorPosition;
+import org.robockets.robot.elevator.TimedElevator;
 
 /**
  * @author Mathias Kools, Jake Backer
@@ -33,7 +35,7 @@ public MidAuto(AutoHelper.RobotPosition robotPosition, AutoHelper.Priority prior
 			switch (robotPosition) {
 				case MIDDLE: // If in position 2 (middle)
 					// Drive to our switch position
-					dropCubeMiddleToSwitch(teamSwitchLeft);
+					dropCubeMiddleToSwitch(teamSwitchLeft); // Tested
 					break;
 				case LEFT: // Else if in position 1 (left) or 3 (right)
 				case RIGHT: // These two enums are just here for readability & changeability.
@@ -42,46 +44,51 @@ public MidAuto(AutoHelper.RobotPosition robotPosition, AutoHelper.Priority prior
 						case SCALE: //If priority is scale.
 							if (scaleSameSide) { // If scale is on our side
 								// Deposit cube in scale
-								dropCubeInSameSideScale(teamSwitchLeft);
+								dropCubeInSameSideScale(scaleLeft); // TODO: Test
 							} else { // scale is on other side
 								if (teamSwitchSameSide == false) { // If switch is on other side
 									// Drive in S shape to other side
-									dropCubeInOppositeSideScaleSShape(teamSwitchLeft);
+									dropCubeInOppositeSideScaleSShape(teamSwitchLeft); // TODO: Test
 								} else { // If switch is on our side
 									// We can just drop in switch or still drive in s
+									dropCubeInSameSideScale(scaleLeft);
 								}
 							}
 							break;
 						case SWITCH: // Else If priority is switch
 							if (teamSwitchSameSide) { // If switch is on our side
 								// Deposit cube in switch.
-								dropCubeInSameSideSwitch(teamSwitchLeft);
+								dropCubeInSameSideSwitch(teamSwitchLeft); // Tested
 							} else { // Else
 								// If scale is on our side
 								if (teamSwitchSameSide) {
 									// Deposit cube in scale
-									dropCubeInSameSideScale(teamSwitchLeft);
+									dropCubeInSameSideScale(scaleLeft); // TODO: Test
 								} else { // Else
 									// Drive to auto line.
-									autoLine();
+									autoLine(); // Tested
 								}
 							}
 							break;
 						default: // Else
-							if (scaleSameSide) { // If scale is on our side
-								// Deposit cube in scale
-								dropCubeInSameSideScale(teamSwitchLeft);
-							} else if (teamSwitchSameSide) { // Else if switch is on our side
+							if (teamSwitchSameSide) { // If switch is on our side
 								// Deposit cube in switch
-								dropCubeInSameSideSwitch(teamSwitchLeft);
+								dropCubeInSameSideSwitch(teamSwitchLeft); // Tested
+							} else if (scaleSameSide) { // Else if scale is on our side
+								/*if(teamSwitchSameSide) {
+									twoCubeSS(teamSwitchLeft);
+								} else {*/
+								// Deposit cube in scale
+								dropCubeInSameSideScale(scaleLeft); // TODO: Test
+								//}
 							} else { // Else
 								// Drive to auto line.
-								autoLine();
+								autoLine(); // Tested
 							}
 					}
 			}
 		} else {
-			autoLine();
+			autoLine(); // Tested
 		}
 	}
 
@@ -100,16 +107,35 @@ private void dropCube() {
 		addSequential(new DropCube());
 	}
 
+	private void twoCubeSS(boolean scaleLeft) {
+		addParallel(new Elevate(ElevatorPosition.MID_SCALE));
+		dropCubeInSameSideScale(scaleLeft);
+		addSequential(new TurnAbsolute(0));
+		driveStraight(-195); // Either this
+		//driveStraight(150); // or this
+		addParallel(new Elevate(ElevatorPosition.BOTTOM));
+		turnAngle(135);
+		addParallel(new IntakeCube(0.75, 0.4, 0.65, 4));
+		driveStraight(50); // TODO: Make this an actual value
+		driveStraight(-50); // TODO: This may or may not work
+		addSequential(new TurnAbsolute(0));
+		driveStraight(-10); // TODO: This will change
+		addParallel(new Elevate(ElevatorPosition.SWITCH));
+		turnAngle(90);
+		driveStraight(50);
+	}
+
 	// Small bit of repeated code that could be put into a method.
 	// Starting position has the corner of robot right where slant of corner of field starts.
 	private void dropCubeInSameSideSwitch(boolean teamSwitchLeft) {
 		//addParallel(new Elevate(ElevatorPosition.SWITCH));
 
 		System.out.println("SSS");
 		// Deposit cube in switch
-		driveStraight(140);
+		driveStraight(130); // FIXME: This will be 140
 		// 90deg CW if on the left.
-		turnAngle(teamSwitchLeft ? -90 : 90);
+		addSequential(new TurnAbsolute(teamSwitchLeft ? -90 : 90));
+		addSequential(new WaitCommand(0.5));
 		// align edge of robot to edge of switch.
 		driveStraight(16.56);
 		//dropCube();
@@ -123,16 +149,16 @@ private void autoLine() {
 
 	// Small bit of repeated code for going to the same side scale and dropping a cube.
 	// Starting position the same as dropCubeInSameSideSwitch.
-	private void dropCubeInSameSideScale(boolean teamSwitchLeft) {
-		addParallel(new Elevate(ElevatorPosition.MID_SCALE));
+	private void dropCubeInSameSideScale(boolean scaleLeft) {
+		//addParallel(new Elevate(ElevatorPosition.MID_SCALE));
 
 		// Deposit cube in scale.
 		// Drive straight the distance to the scale.
 		driveStraight(305.15);
 		// 90deg CW if on the left.
-		turnAngle(teamSwitchLeft ? 90 : -90);
+		turnAngle(scaleLeft ? 90 : -90);
 		driveStraight(2.88);
-		dropCube();
+		//dropCube();
 	}
 
 	// Code that is so ugly it should be tucked away here. FIXME: This is still super broken
@@ -143,7 +169,19 @@ private void dropCubeInOppositeSideScaleSShape(boolean teamSwitchLeft) {
 		double angle = teamSwitchLeft ? 90 : -90;
 		turnAngle(angle); // turn 90deg CW when on the left.
 		driveStraight(244.5);
-		turnAngle(-angle); // 90deg CCW when starting from the left.
+		turnAngle(-angle); // 90deg CCW whaddParallel(new Elevate(ElevatorPosition.SWITCH));
+
+		driveStraight(36); // 50 of all the numbers was chosen arbitrarily.
+		addSequential(new WaitCommand(0.1));
+		double smallAngle = (teamSwitchLeft ? 40 : -42.5); // CW first on the left. TODO: Change these
+		//turnAngle(smallAngle);
+		addSequential(new TurnAbsolute(smallAngle));
+		addSequential(new WaitCommand(0.1));
+		driveStraight(68);
+		addSequential(new WaitCommand(0.1));
+		//turnAngle(-smallAngle);
+		addSequential(new TurnAbsolute(0));
+		addSequential(new WaitCommand(0.1)); //en starting from the left.
 		driveStraight(98.915);
 		turnAngle(-angle);
 		driveStraight(2.88);
@@ -154,20 +192,20 @@ private void dropCubeInOppositeSideScaleSShape(boolean teamSwitchLeft) {
 	// of the exchange to the side of the robot.
 	private void dropCubeMiddleToSwitch(boolean teamSwitchLeft) {
 		//addParallel(new Elevate(ElevatorPosition.SWITCH));
+		//addParallel(new TimedElevator(3.75, 0.8));
 
-		driveStraight(36); // 50 of all the numbers was chosen arbitrarily.
-		addSequential(new WaitCommand(0.1));
-		double smallAngle = (teamSwitchLeft ? 41.7 : -41.7); // CW first on the left.
+		driveStraight(25); // 50 of all the numbers was chosen arbitrarily.
+
+		double smallAngle = (teamSwitchLeft ? 42.5 : -40.5); // CW first on the left.
 		//turnAngle(smallAngle);
 		addSequential(new TurnAbsolute(smallAngle));
-		addSequential(new WaitCommand(0.1));
-		driveStraight(68);
-		addSequential(new WaitCommand(0.1));
+
+		driveStraight(teamSwitchLeft ? 90 : 70);
+
 		//turnAngle(-smallAngle);
 		addSequential(new TurnAbsolute(0));
-		addSequential(new WaitCommand(0.1));
-		driveStraight(8);
-		//dropCube();
 
+		driveStraight(21);
+		//dropCube();
 	}
 }

src/org/robockets/robot/autonomous/TestAuto.java

@@ -4,15 +4,16 @@
 import edu.wpi.first.wpilibj.command.WaitCommand;
 import org.robockets.robot.drivetrain.DriveAngleAssisted;
 import org.robockets.robot.drivetrain.DriveStraightAssisted;
+import org.robockets.robot.drivetrain.TurnAbsolute;
 
 /**
  * @author Jake Backer
  */
 public class TestAuto extends CommandGroup {
 
 	public TestAuto() {
-		addSequential(new DriveStraightAssisted(36));
-		addSequential(new WaitCommand(0.2));
-		addSequential(new DriveStraightAssisted(-36));
+		addSequential(new DriveStraightAssisted(120));
+		addSequential(new TurnAbsolute(90));
+		addSequential(new DriveStraightAssisted(36)); // BE CAREFUL WITH THIS
 	}
 }