Added more framework stuff

.classpath

@@ -11,6 +11,5 @@
 	<classpathentry kind="var" path="USERLIBS_DIR/CTRE_Phoenix.jar"/>
 	<classpathentry kind="var" path="USERLIBS_DIR/navx_frc.jar"/>
 	<classpathentry kind="var" path="USERLIBS_DIR/Pathfinder-Java-1.8.jar"/>
-	<classpathentry kind="var" path="USERLIBS_DIR/Pathfinder-Java.jar"/>
 	<classpathentry kind="output" path="bin"/>
 </classpath>

src/org/usfirst/frc/team2403/robot/Elevator.java

@@ -213,7 +213,7 @@ public void elevatorLift(double speed) {
 			leftLift.setSelectedSensorPosition(0, 0, 0);
 		}
 
-		else if(speed > 0 && getLiftLimit(getPivotAngle()) + .5 < getLiftDistance()) {
+		else if(speed > 0 && (getLiftLimit(getPivotAngle()) + .5 < getLiftDistance() || getLiftLimit(pivotPosToAngle(pivotTarget)) + .5 < getLiftDistance())) {
 			liftSpeed = 0;
 			activateLiftBrake();
 		}

src/org/usfirst/frc/team2403/robot/Robot.java

@@ -16,7 +16,8 @@
 //import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 //import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
+import edu.wpi.cscore.UsbCamera;
+import edu.wpi.first.wpilibj.CameraServer;
 
 public class Robot extends IterativeRobot {
 
@@ -62,19 +63,18 @@ public void robotInit() {
 		compressor.setClosedLoopControl(true);
 
 		autoModeRunner = new AutoModeRunner();
-		autoModes = new AutoMode[4];
+		autoModes = new AutoMode[20];
 		for(int i = 0; i < autoModes.length; i++){
 			autoModes[i] = new Nothing();
 		}
 
 		autoModeSelection = 0;
 		SmartDashboard.putNumber("Auto Mode", 0);
 
-		//m_chooser.addDefault("Default Auto", kDefaultAuto);
-		//m_chooser.addObject("My Auto", kCustomAuto);
-		//SmartDashboard.putData("Auto choices", m_chooser);
-
 		driveTrain.resetEncoders();
+
+		CameraServer.getInstance().startAutomaticCapture().setResolution(320, 240);
+		//Troy loves cameras
 	}
 
 	public void robotPeriodic() {
@@ -104,12 +104,13 @@ public void autonomousInit() {
 		autoModes[1] = new RightSwitch(driveTrain, intake);
 		autoModes[2] = new LeftSwitch(driveTrain, intake);
 		autoModes[3] = new CenterSwitch(driveTrain, intake, elevator);
+		autoModes[4] = new LeftScale(driveTrain, intake);
 
 		autoModeSelection = (autoModeSelection >= autoModes.length) ? 0 : autoModeSelection;
 		autoModeSelection = (autoModeSelection < 0) ? 0 : autoModeSelection;
 		autoModeRunner.chooseAutoMode(autoModes[autoModeSelection]);
 
-		autoModeRunner.chooseAutoMode(new LeftScale(driveTrain, intake));
+		//autoModeRunner.chooseAutoMode(new TrajectoryTest(driveTrain, elevator, intake));
 
 		autoModeRunner.start();
 
@@ -207,9 +208,6 @@ else if(joystick.L3.isPressed()) {
 	@Override
 	public void testInit() {
 		driveTrain.FPSDrive(0, 0);
-		autoModeRunner.chooseAutoMode(new GenerateTrajectories());
-
-		autoModeRunner.start();
 	}
 
 	@Override

src/org/usfirst/frc/team2403/robot/auto/actions/FollowTrajectory.java

@@ -52,8 +52,8 @@ public FollowTrajectory(String name, DriveTrain drive) {
 		this.drive = drive;
 		this.followLoop = new Notifier(new PeriodicRunnable());
 
-		File leftFile = new File("/home/lvuser/" + name + "Left");
-		File rightFile = new File("/home/lvuser/" + name + "Right");
+		File leftFile = new File("/media/sda1/" + name + "Left");
+		File rightFile = new File("/media/sda1/" + name + "Right");
 		Trajectory left = Pathfinder.readFromFile(leftFile);
 		Trajectory right = Pathfinder.readFromFile(rightFile);
 

src/org/usfirst/frc/team2403/robot/auto/modes/CenterSwitch.java

@@ -0,0 +1,69 @@
+
+package org.usfirst.frc.team2403.robot.auto.modes;
+
+import org.usfirst.frc.team2403.robot.Constants;
+import org.usfirst.frc.team2403.robot.DriveTrain;
+import org.usfirst.frc.team2403.robot.Elevator;
+import org.usfirst.frc.team2403.robot.Intake;
+import org.usfirst.frc.team2403.robot.auto.actions.Clamp;
+import org.usfirst.frc.team2403.robot.auto.actions.FollowTrajectory;
+import org.usfirst.frc.team2403.robot.auto.actions.IntakeCube;
+import org.usfirst.frc.team2403.robot.auto.actions.PivotAngle;
+import org.usfirst.frc.team2403.robot.auto.actions.Wait;
+import org.usfirst.frc.team2403.robot.auto.util.AutoMode;
+import org.usfirst.frc.team2403.robot.auto.util.AutoModeEndedException;
+
+import edu.wpi.first.wpilibj.DriverStation;
+
+public class CenterSwitch extends AutoMode{ 
+
+	DriveTrain drive;
+	Intake intake;
+	Elevator elevator;
+
+	String gameData;
+
+	public CenterSwitch(DriveTrain drive, Intake intake, Elevator elevator) {
+		this.drive = drive;
+		this.intake = intake;
+		this.elevator = elevator;
+		gameData = DriverStation.getInstance().getGameSpecificMessage();
+	}
+
+
+	@Override
+	protected void routine() throws AutoModeEndedException {		
+		if(gameData.charAt(0) == 'R') {
+			runAction(new Clamp(intake, true));
+			runAction(new FollowTrajectory("SwitchCenterRight1", drive));
+			runAction(new IntakeCube(.7, false, intake));
+			runActionsParallel(new PivotAngle(Constants.PIVOT_POSITION_BOTTOM, elevator), new FollowTrajectory("SwitchCenterRight2", drive));
+			runAction(new IntakeCube(.8, true, intake));
+			runAction(new Clamp(intake, false));
+			runAction(new FollowTrajectory("SwitchCenterRight3", drive));
+			runAction(new Clamp(intake, true));
+			runAction(new IntakeCube(0, true, intake));
+			runActionsParallel(new PivotAngle(Constants.PIVOT_POSITION_SWITCH, elevator), new FollowTrajectory("SwitchCenterRight4", drive));
+			runAction(new FollowTrajectory("SwitchCenterRight5", drive));
+			runAction(new IntakeCube(.6, false, intake));
+			runAction(new Wait(1));
+			runAction(new IntakeCube(0, true, intake));
+		} 
+		else {
+			runAction(new Clamp(intake, true));
+			runAction(new FollowTrajectory("CenterSwitchLeft1", drive));
+			runAction(new IntakeCube(.7, false, intake));
+			runActionsParallel(new PivotAngle(Constants.PIVOT_POSITION_BOTTOM, elevator), new FollowTrajectory("CenterSwitchLeft2", drive));
+			runAction(new IntakeCube(.8, true, intake));
+			runAction(new Clamp(intake, false));
+			runAction(new FollowTrajectory("CenterSwitchLeft3", drive));
+			runAction(new Clamp(intake, true));
+			runAction(new IntakeCube(0, true, intake));
+			runActionsParallel(new PivotAngle(Constants.PIVOT_POSITION_SWITCH, elevator), new FollowTrajectory("CenterSwitchLeft4", drive));
+			runAction(new FollowTrajectory("CenterSwitchLeft5", drive));
+			runAction(new IntakeCube(.6, false, intake));
+			runAction(new Wait(1));
+			runAction(new IntakeCube(0, true, intake));
+		}
+	}
+}

src/org/usfirst/frc/team2403/robot/auto/modes/LeftScale.java

@@ -0,0 +1,39 @@
+
+package org.usfirst.frc.team2403.robot.auto.modes;
+
+import org.usfirst.frc.team2403.robot.DriveTrain;
+import org.usfirst.frc.team2403.robot.Intake;
+import org.usfirst.frc.team2403.robot.auto.actions.DriveStraight;
+import org.usfirst.frc.team2403.robot.auto.actions.FollowTrajectory;
+import org.usfirst.frc.team2403.robot.auto.actions.IntakeCube;
+import org.usfirst.frc.team2403.robot.auto.actions.TurnAngle;
+import org.usfirst.frc.team2403.robot.auto.util.AutoMode;
+import org.usfirst.frc.team2403.robot.auto.util.AutoModeEndedException;
+
+import edu.wpi.first.wpilibj.DriverStation;
+
+public class LeftScale extends AutoMode{ 
+
+	DriveTrain drive;
+	Intake intake;
+
+	String gameData;
+
+	public LeftScale(DriveTrain drive, Intake intake) {
+		this.drive = drive;
+		this.intake = intake;
+		gameData = DriverStation.getInstance().getGameSpecificMessage();
+	}
+
+
+	@Override
+	protected void routine() throws AutoModeEndedException {
+		if(gameData.charAt(0) == 'L') {
+			runAction(new FollowTrajectory("LeftScaleLeft1", drive));
+		}
+
+		else {
+
+		}
+	}
+}