Updated snapshot as of 20180305-1715

src/org/usfirst/frc4388/robot/Constants.java

@@ -21,13 +21,21 @@ public class Constants {
     public static double kDriveLowGearMaxSpeedInchesPerSec = 12.0 * 5.0;
     public static double kDriveStraightBasicMaxSpeedInchesPerSec = 72.0;
     public static double kDriveStraightBasicMinSpeedInchesPerSec = 5.0;
-    public static double kDriveTurnBasicTankMotorOutput = 0.2;
+    public static double kDriveTurnBasicTankMotorOutput = 0.5;
     public static double kDriveTurnBasicSingleMotorOutput = 0.15;
     public static double kElevatorWheelDiameterInches = 1;
 	// Encoders
 	public static int kDriveEncoderTicksPerRev = 4096;
 	public static double kDriveEncoderTicksPerInch = (double)kDriveEncoderTicksPerRev / (kDriveWheelDiameterInches * Math.PI);
-	public static double kElevatorEncoderTicksPerInch = (double)kDriveEncoderTicksPerRev /(kElevatorWheelDiameterInches * Math.PI) ;
+	
+	// Elevator
+	public static int kElevatorEncoderTickesPerRev = 256;
+    public static double kElevatorInchesOfTravelPerRev = 3.75;
+//	public static double kElevatorEncoderTicksPerInch = (double)kElevatorEncoderTickesPerRev / kElevatorInchesOfTravelPerRev;
+	public static double kElevatorEncoderTicksPerInch = 126.36;
+	public static double kElevatorBasicPercentOutputUp = -.9;
+	public static double kElevatorBasicPercentOutputDown =.9;
+	
     // CONTROL LOOP GAINS
 
     // PID gains for drive velocity loop (LOW GEAR)

src/org/usfirst/frc4388/robot/OI.java

@@ -187,7 +187,10 @@ public class OI
 			//SmartDashboard.putData("move intake", new IntakePosition(true));
 			//SmartDashboard.putData("move intake2", new IntakePosition(false));
 			//SmartDashboard.putData("Move Stop", new ElevatorSetSpeed(ElevatorAuton.RAISE_ELEVATOR_SPEED));
-			SmartDashboard.putData("move elevator", new ElevatorBasic(20, 15));
+	        
+	        
+			SmartDashboard.putData("move elevator", new ElevatorBasic(20));
+	        
 			///SmartDashboard.putData("Move Down", new ElevatorSetSpeed(ElevatorAuton.LOWER_ELEVATOR_SPEED));
 			///SmartDashboard.putData("Move UP", new ElevatorSetSpeed(ElevatorAuton.STOP_ELEVATOR_SPEED));
 			

src/org/usfirst/frc4388/robot/Robot.java

@@ -33,7 +33,8 @@ import org.usfirst.frc4388.robot.subsystems.*;
 import org.usfirst.frc4388.utility.ControlLooper;
 import org.usfirst.frc4388.robot.subsystems.Drive;
 import org.usfirst.frc4388.robot.subsystems.Carriage;
-import org.usfirst.frc4388.robot.subsystems.LED;;
+import org.usfirst.frc4388.robot.subsystems.LED;
+import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;;
 /**
  * The VM is configured to automatically run this class, and to call the
  * functions corresponding to each mode, as described in the TimedRobot
@@ -50,7 +51,6 @@ public class Robot extends IterativeRobot
 
 	//public static final Elevator elevator = new Elevator();
     public static final Carriage carriage = new Carriage();
-    public static final ElevatorAuton elevatorAuton = new ElevatorAuton();
     public static final Elevator elevator = new Elevator();
     
     
@@ -72,9 +72,15 @@ public class Robot extends IterativeRobot
 	public static OperationMode operationMode = OperationMode.TEST;
 
 	private SendableChooser<OperationMode> operationModeChooser;
-	private SendableChooser<Command> autonTaskChooser;
+	private SendableChooser<Command> RRautonTaskChooser;
+	private SendableChooser<Command> RLautonTaskChooser;
+	private SendableChooser<Command> LRautonTaskChooser;
+	private SendableChooser<Command> LLautonTaskChooser;
 
-    private Command autonomousCommand;
+    private Command RRautonomousCommand;
+    private Command RLautonomousCommand;
+    private Command LRautonomousCommand;
+    private Command LLautonomousCommand;
     
     /**
      * This function is run when the robot is first started up and should be
@@ -109,14 +115,61 @@ public class Robot extends IterativeRobot
 	    operationModeChooser.addObject("Competition", OperationMode.COMPETITION);
 		SmartDashboard.putData("Operation Mode", operationModeChooser);
 		
-		autonTaskChooser = new SendableChooser<Command>();
-
-		//autonTaskChooser.addDefault("Blue Gear Loading Side Plus Travel Forward", new BlueGearLoadingSideForwardAuton());
-		autonTaskChooser.addObject("Right Switch", new RightSwitchAuton());
-		//autonTaskChooser.addObject("Right Switch Forward", new RightSwitchForward());
-		autonTaskChooser.addDefault("RightStartLeftScore", new RightStartLeftScore());
 		
-		SmartDashboard.putData("Auton Task", autonTaskChooser);
+		
+		
+		
+		RRautonTaskChooser = new SendableChooser<Command>();
+		
+		RRautonTaskChooser.addObject("Right Switch", new RightSwitchAuton());
+		RRautonTaskChooser.addDefault("RR Cross The Base Line", new CrossTheBaseLine());
+		RRautonTaskChooser.addObject("LeftSwitch Center", new CenterLeft());
+
+		
+		SmartDashboard.putData("RRAuton Task", RRautonTaskChooser);
+		
+		
+		
+		
+		
+		RLautonTaskChooser = new SendableChooser<Command>();
+		
+		RLautonTaskChooser.addObject("Right Switch", new RightSwitchAuton());
+		RLautonTaskChooser.addDefault("RL Cross The Base Line", new CrossTheBaseLine());
+		RLautonTaskChooser.addObject("LeftSwitch Center", new CenterLeft());
+
+		
+		SmartDashboard.putData("RLAuton Task", RLautonTaskChooser);
+		
+		
+		
+		
+		
+		
+		LLautonTaskChooser = new SendableChooser<Command>();
+		
+		LLautonTaskChooser.addObject("Right Switch", new RightSwitchAuton());
+		LLautonTaskChooser.addDefault("LL Cross The Base Line", new CrossTheBaseLine());
+		LLautonTaskChooser.addObject("LeftSwitch Center", new CenterLeft());
+
+		
+		SmartDashboard.putData("LLAuton Task", LLautonTaskChooser);
+		
+		
+		
+		
+		
+		LRautonTaskChooser = new SendableChooser<Command>();
+		
+		LRautonTaskChooser.addObject("Right Switch", new RightSwitchAuton());
+		LRautonTaskChooser.addDefault("LR Cross The Base Line", new CrossTheBaseLine());
+		LRautonTaskChooser.addObject("LeftSwitch Center", new CenterLeft());
+
+		
+		SmartDashboard.putData("LRAuton Task", LRautonTaskChooser);
+
+
+
 
 		
 		//ledLights.setAllLightsOn(false);
@@ -158,14 +211,36 @@ public class Robot extends IterativeRobot
     	//elevator.resetElevatorEncoder();
     	drive.resetGyro();
     	drive.setIsRed(getAlliance().equals(Alliance.Red));
-        if (autonomousCommand != null) autonomousCommand.start();
+        
-    }
+        
-
+    	String gameData;
-    /**
+		gameData = DriverStation.getInstance().getGameSpecificMessage();
-     * This function is called periodically during autonomous
+                if(gameData.length() > 0)
-     */
+                {
+		  if(gameData.charAt(0) == 'L')
+		  {
+			  if(gameData.charAt(1) == 'L')
+			  {
+				  if (LLautonomousCommand != null) LLautonomousCommand.start();
+			  } else {
+				  if (LRautonomousCommand != null) LRautonomousCommand.start();
+			  }
+	                }
+		  } else {
+			  if(gameData.charAt(1) == 'L')
+			  {
+				  if (RLautonomousCommand != null) RLautonomousCommand.start();
+			  } else {
+				  if (RRautonomousCommand != null) RRautonomousCommand.start();
+			  }
+	                }
+			
+		  }
+                
+    
     public void autonomousPeriodic() {
         Scheduler.getInstance().run();
+        //Robot.elevator.setControlMode(DriveControlMode.RAW);
 		updateStatus();
     }
 
@@ -174,7 +249,10 @@ public class Robot extends IterativeRobot
         // teleop starts running. If you want the autonomous to 
         // continue until interrupted by another command, remove
         // this line or comment it out.
-        if (autonomousCommand != null) autonomousCommand.cancel();
+        if (RRautonomousCommand != null) RRautonomousCommand.cancel();
+        if (RLautonomousCommand != null) RLautonomousCommand.cancel();
+        if (LRautonomousCommand != null) LRautonomousCommand.cancel();
+        if (LLautonomousCommand != null) LLautonomousCommand.cancel();
         //MotionProfileCache.getInstance().release();
     	updateChoosers();
         controlLoop.start();
@@ -185,6 +263,7 @@ public class Robot extends IterativeRobot
     	//shooterFeed.setSpeed(0);
     	//zarkerFeed.setSpeed(0);
     	//shooter.setHopperPosition(HopperState.CLOSE);
+    	//Robot.elevator.setControlMode(DriveControlMode.JOYSTICK);
         updateStatus();
     }
 
@@ -207,7 +286,10 @@ public class Robot extends IterativeRobot
     
     private void updateChoosers() {
 		operationMode = (OperationMode)operationModeChooser.getSelected();
-		autonomousCommand = (Command)autonTaskChooser.getSelected();
+		RRautonomousCommand = (Command)RRautonTaskChooser.getSelected();
+		RLautonomousCommand = (Command)RLautonTaskChooser.getSelected();
+		LRautonomousCommand = (Command)LRautonTaskChooser.getSelected();
+		LLautonomousCommand = (Command)LLautonTaskChooser.getSelected();
     }
     
     public Alliance getAlliance() {
@@ -216,6 +298,7 @@ public class Robot extends IterativeRobot
     
     public void updateStatus() {
     	drive.updateStatus(operationMode);
+    	elevator.updateStatus(operationMode);
     	//carriage.updateStatus(operationMode);
     	//shooter.updateStatus(operationMode);
     	//shooterFeed.updateStatus(operationMode);

src/org/usfirst/frc4388/robot/commands/ElevatorBasic.java

@@ -10,36 +10,36 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 
 public class ElevatorBasic extends Command {
-    private double m_targetInches;
+    private double m_targetHeightInchesAboveFloor;
-    private double m_maxVelocityInchesPerSec;
     private double m_speed;
-    private boolean m_goingBackwards;
+    private boolean m_goingUp;
     private double m_commandInitTimestamp;
 	private double m_lastCommandExecuteTimestamp = 0.0;
 	private double m_lastCommandExecutePeriod = 0.0;
+	public static boolean isfinishedElevatorBasic;
  
-    public ElevatorBasic(double targetInches, double maxVelocityInchesPerSec) {
+    public ElevatorBasic(double targetHeightInchesAboveFloor) {
         requires(Robot.elevator);
-        m_targetInches = targetInches;
+        m_targetHeightInchesAboveFloor = targetHeightInchesAboveFloor;
-        m_maxVelocityInchesPerSec = maxVelocityInchesPerSec;
-        m_goingBackwards = (m_targetInches < 0.0);
    }
-    
-    protected double velocityToMoveSpeed(double velocityInchesPerSec, boolean backwards) {
-    	double sign = (backwards ? -1.0 : 1.0);
-    	// Keep velocity above stiction limit (else bot will freeze and command will not complete)
-    	double velocity = Math.max(Constants.kDriveStraightBasicMinSpeedInchesPerSec, velocityInchesPerSec);
-        // Figure out move value based on percentage of measured max speed (i.e. that at full 1.0 joystick)
-    	return sign * velocity / Constants.kDriveStraightBasicMaxSpeedInchesPerSec;
-    }
 
     // Called just before this Command runs the first time
-    protected void initialize() {
+    protected void initialize() 
-    	Robot.elevator.resetElevatorEncoder();
+    {
     	Robot.elevator.setControlMode(DriveControlMode.RAW);
+    	
+    	double currentHeight = Robot.elevator.getElevatorHeightInchesAboveFloor();
     	// start out at half speed, as crude way to reduce slippage
-        m_speed = velocityToMoveSpeed(m_maxVelocityInchesPerSec/2.0, m_goingBackwards);
+    	m_goingUp = (m_targetHeightInchesAboveFloor > currentHeight);
+System.out.println("initialize(): cur=" + currentHeight + " , target=" + m_targetHeightInchesAboveFloor + " , going " + (m_goingUp ? "UP" : "DOWN"));
+    	if (m_goingUp) {
+    		m_speed = Constants.kElevatorBasicPercentOutputUp;
+    	}
+    	else {
+    		m_speed = Constants.kElevatorBasicPercentOutputDown;
+    	}
 		m_commandInitTimestamp = Timer.getFPGATimestamp();
+		
     }
 
     // Called repeatedly when this Command is scheduled to run
@@ -50,31 +50,31 @@ public class ElevatorBasic extends Command {
 			m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp;
 		}
 		m_lastCommandExecuteTimestamp = currentTimestamp;
-    	double steer = 0.0;
+    	Robot.elevator.rawSetOutput(m_speed);
-
-    	
-    	Robot.elevator.rawMoveSteer(m_speed, steer);
 		//SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod);
     }
 
     // Make this return true when this Command no longer needs to run execute()
     protected boolean isFinished() {
     	boolean finished=false;
-    	double velocity = m_maxVelocityInchesPerSec;
+    	double currentHeight = Robot.elevator.getElevatorHeightInchesAboveFloor();
-    	double position = (Robot.elevator.getelevatorPositionWorld());
+    	double remaining = (m_targetHeightInchesAboveFloor - currentHeight) * (m_goingUp ? 1.0 : -1.0);
-    	double remaining = (m_targetInches - position) * (m_goingBackwards ? -1.0 : 1.0);
+System.out.println("cur=" + currentHeight + " , remaining=" + remaining + " , target=" + m_targetHeightInchesAboveFloor);
     	if (remaining < 0.0) {
     		finished = true;
-    	} else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) {	// last 100 ms
+    		
+    	}
+    	/*} else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) {	// last 100 ms
     		velocity = m_maxVelocityInchesPerSec / 4.0;		// quarter speed
     	} else if (remaining < 0.3 * m_maxVelocityInchesPerSec) {		// last 300 ms
     		velocity = m_maxVelocityInchesPerSec / 2.0;		// half speed
-    	}
+    	}*/
+    		
+    		
     	if (!finished) {
-    		m_speed = velocityToMoveSpeed(velocity, m_goingBackwards);
+    		SmartDashboard.putNumber("EB Dist", currentHeight);
-    		SmartDashboard.putNumber("DSB Dist", position);
     	} else {
-    		SmartDashboard.putNumber("DSB finDist", position);
+    		SmartDashboard.putNumber("EB finDist", currentHeight);
     	}
 		return finished;
     }
@@ -82,8 +82,12 @@ public class ElevatorBasic extends Command {
     // Called once after isFinished returns true
     protected void end() {
 		double currentTimestamp = Timer.getFPGATimestamp();
-    	SmartDashboard.putNumber("DSB Runtime", currentTimestamp - m_commandInitTimestamp);
+    	SmartDashboard.putNumber("EB Runtime", currentTimestamp - m_commandInitTimestamp);
-    	Robot.elevator.rawMoveSteer(0.0, 0.0);
+    	
+    	isfinishedElevatorBasic = isFinished();
+    	
+    	Robot.elevator.rawSetOutput(0.0);
+    	
 		Robot.elevator.setControlMode(DriveControlMode.JOYSTICK);
     }
 

src/org/usfirst/frc4388/robot/commands/ElevatorSetDeploy.java

@@ -2,7 +2,7 @@ package org.usfirst.frc4388.robot.commands;
 
 import org.usfirst.frc4388.robot.Robot;
 import org.usfirst.frc4388.robot.commands.IntakeSetPosition.IntakePosition;
-import org.usfirst.frc4388.robot.subsystems.ElevatorAuton;
+//import org.usfirst.frc4388.robot.subsystems.ElevatorAuton;
 
 import edu.wpi.first.wpilibj.command.Command;
 
@@ -30,12 +30,12 @@ public class ElevatorSetDeploy extends Command {
     	secondTimerSet = false;
     	if (position == IntakePosition.CUBE_INTAKE) {
         	this.setTimeout(timeout);
-    		Robot.elevatorAuton.setRaiseSpeed(ElevatorAuton.LOWER_ELEVATOR_SPEED);
+    		//Robot.elevatorAuton.setRaiseSpeed(ElevatorAuton.LOWER_ELEVATOR_SPEED);
     		isDeploy = true;
     	}
     	else if (position == IntakePosition.CUBE_DEPLOY) {
         	this.setTimeout(timeout);
-    		Robot.elevatorAuton.setRaiseSpeed(ElevatorAuton.RAISE_ELEVATOR_SPEED);
+    		//Robot.elevatorAuton.setRaiseSpeed(ElevatorAuton.RAISE_ELEVATOR_SPEED);
     		isDeploy = true;
     	}
     	else {
@@ -63,7 +63,7 @@ public class ElevatorSetDeploy extends Command {
     // Called once after isFinished returns true
     protected void end() {
     	if (isDeploy == true) {
-    		Robot.elevatorAuton.setRaiseSpeed(0);
+    		//Robot.elevatorAuton.setRaiseSpeed(0);
     	}
     }
 

src/org/usfirst/frc4388/robot/commands/ElevatorSetSpeed.java

@@ -16,12 +16,12 @@ public class ElevatorSetSpeed extends Command {
 	// Constructor with speed
     public ElevatorSetSpeed(double RaiseSpeed) {
     	this.RaiseSpeed = RaiseSpeed;
-        requires(Robot.elevatorAuton);
+       // requires(Robot.elevatorAuton);
     }
 
     // Called just before this Command runs the first time
     protected void initialize() {
-    	Robot.elevatorAuton.setRaiseSpeed(RaiseSpeed);
+    	//Robot.elevatorAuton.setRaiseSpeed(RaiseSpeed);
     }
 
     // Called repeatedly when this Command is scheduled to run

src/org/usfirst/frc4388/robot/commands/auton/BlueGearLoadingSideForwardAuton.java

@@ -1,49 +0,0 @@
-
-package org.usfirst.frc4388.robot.commands.auton;
-
-import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP;
-import org.usfirst.frc4388.robot.commands.DriveGyroReset;
-//import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
-import org.usfirst.frc4388.robot.commands.DriveStraightMP;
-//import org.usfirst.frc4388.robot.commands.IntakeSetPosition;
-//import org.usfirst.frc4388.robot.commands.IntakeSetPosition.IntakePosition;
-//import org.usfirst.frc4388.robot.commands.ShooterSetRpm;
-//import org.usfirst.frc4388.robot.commands.ShooterSetShotPosition;
-//import org.usfirst.frc4388.robot.commands.ShooterSetVoltageRampRate;
-import org.usfirst.frc4388.robot.subsystems.Drive;
-//import org.usfirst.frc4388.robot.subsystems.Shooter;
-//import org.usfirst.frc4388.robot.subsystems.Drive.SpeedShiftState;
-//import org.usfirst.frc4388.robot.subsystems.Shooter.ShotState;
-import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-import edu.wpi.first.wpilibj.command.WaitCommand;
-
-/**
- *
- */
-public class BlueGearLoadingSideForwardAuton extends CommandGroup {
-    
-    public BlueGearLoadingSideForwardAuton() {
-    	addSequential(new DriveGyroReset());
-    	//addSequential(new IntakeSetPosition(IntakePosition.GEAR_PRESENT));
-
-    	//addSequential(new DriveStraightMP(60, 10, true, true, 0));  // 95 for 112" greenville
-        //addSequential(new DriveAbsoluteTurnMP(10, Drive.MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC, MPSoftwareTurnType.TANK));
-        //addSequential(new DriveStraightMP(10, Drive.MP_GEAR_DEPLOY_FASTER_VELOCITY_INCHES_PER_SEC, true, true, -30));
-        //addSequential(new IntakeSetPosition(IntakePosition.GEAR_DEPLOY));
-       // addSequential(new WaitCommand(1.0));
-    	//addSequential(new DriveStraightMP(40, Drive.MP_AUTON_MAX_STRAIGHT_VELOCITY_INCHES_PER_SEC, true, true, -10));  // 95 for 112" greenville
-        //addSequential(new DriveAbsoluteTurnMP(10, Drive.MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC, MPSoftwareTurnType.TANK));
-       /// addSequential(new DriveStraightMP(-10, Drive.MP_GEAR_DEPLOY_FASTER_VELOCITY_INCHES_PER_SEC, true, true, -30));
-        
-       // addSequential(new DriveStraightMP(40, Drive.MP_AUTON_MAX_STRAIGHT_VELOCITY_INCHES_PER_SEC, true, true, 10));
-        //addSequential(new IntakeSetPosition(IntakePosition.GEAR_DEPLOY));
-
-       // addSequential(new WaitCommand(0.3));
-       // addSequential(new DriveStraightMP(-20, Drive.MP_AUTON_MAX_BOILER_STRAIGHT_VELOCITY_INCHES_PER_SEC, true, true, -60));
-       // addSequential(new DriveAbsoluteTurnMP(-173, Drive.MP_AUTON_MAX_BOILER_TURN_RATE_DEG_PER_SEC, MPSoftwareTurnType.TANK));
-//        addSequential(new DriveSpeedShift(SpeedShiftState.HI));
-       // addSequential(new DriveStraightMP(-254, Drive.MP_AUTON_MAX_STRAIGHT_VELOCITY_INCHES_PER_SEC, true, true, -173));
-    }
-}

src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java

@@ -6,6 +6,8 @@ import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
 import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
 import org.usfirst.frc4388.robot.commands.DriveStraightMP;
 import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
+import org.usfirst.frc4388.robot.commands.ElevatorBasic;
+import org.usfirst.frc4388.robot.commands.ElevatorSetSpeed;
 import org.usfirst.frc4388.robot.commands.IntakePosition;
 import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
 import org.usfirst.frc4388.robot.subsystems.Carriage;
@@ -18,27 +20,25 @@ import edu.wpi.first.wpilibj.command.WaitCommand;
 /**
  *
  */
-public class RightSwitchForward extends CommandGroup {
+public class CenterLeft extends CommandGroup {
     
-    public RightSwitchForward() {
+    public CenterLeft() {
     	addSequential(new DriveGyroReset());
+    	
     	addSequential(new IntakePosition(true));
-    	addSequential(new DriveStraightBasic(-25, 25, true, true, 0));
+    	addSequential(new DriveStraightBasic(-10, 60, true, true, 0));
-    	addSequential(new DriveTurnBasic(true, 178, 90, MPSoftwareTurnType.TANK));
+    	addSequential(new DriveTurnBasic(true, 130, 300, MPSoftwareTurnType.TANK));
-    	addSequential(new DriveGyroReset());
+    	addSequential(new DriveStraightBasic(50, 60, true, true, 0));
-    	addSequential(new WaitCommand(.1));
+    	addSequential(new DriveTurnBasic(true, 28, 300, MPSoftwareTurnType.TANK));
-    	addSequential(new DriveStraightBasic(125, 30, true, true, 0));
+    	addSequential(new ElevatorBasic(20));
-    	addSequential(new DriveGyroReset());
+    	addSequential(new DriveStraightBasic(25, 60, true, true, 0));
-    	addSequential(new WaitCommand(.1));
-    	addSequential(new DriveTurnBasic(true, -90, 90, MPSoftwareTurnType.TANK));
-    	//addSequential(new DriveStraightBasic(5, 20, true, true, 0));
     	addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
-    	addSequential(new WaitCommand(.6));
+    	//addSequential(new WaitCommand(.1));
     	addSequential(new IntakePosition(false));
-    	addSequential(new WaitCommand(.9));
+    	addSequential(new WaitCommand(.5));
     	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
 
     	//addSequential(new DriveStraightMP(60, 10, true, true, 0));  // 95 for 112" greenville
        
     }
-}
+}

src/org/usfirst/frc4388/robot/commands/auton/CrossTheBaseLine.java

@@ -0,0 +1,34 @@
+package org.usfirst.frc4388.robot.commands.auton;
+
+import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP;
+import org.usfirst.frc4388.robot.commands.DriveGyroReset;
+import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
+import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
+import org.usfirst.frc4388.robot.commands.DriveStraightMP;
+import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
+import org.usfirst.frc4388.robot.commands.ElevatorBasic;
+import org.usfirst.frc4388.robot.commands.IntakePosition;
+import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
+import org.usfirst.frc4388.robot.subsystems.Carriage;
+import org.usfirst.frc4388.robot.subsystems.Drive;
+import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+import edu.wpi.first.wpilibj.command.WaitCommand;
+
+/**
+ *
+ */
+public class CrossTheBaseLine extends CommandGroup {
+    
+    public CrossTheBaseLine() {
+    	
+    	addSequential(new DriveGyroReset());
+    	addSequential(new DriveSpeedShift(true));
+    	addSequential(new DriveSpeedShift(true));
+    	addSequential(new IntakePosition(true));
+    	addSequential(new DriveStraightBasic(-90, 60, true, true, 0));
+
+       
+    }
+}

src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java

@@ -6,6 +6,7 @@ import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
 import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
 import org.usfirst.frc4388.robot.commands.DriveStraightMP;
 import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
+import org.usfirst.frc4388.robot.commands.ElevatorBasic;
 import org.usfirst.frc4388.robot.commands.IntakePosition;
 import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
 import org.usfirst.frc4388.robot.subsystems.Carriage;
@@ -21,19 +22,24 @@ import edu.wpi.first.wpilibj.command.WaitCommand;
 public class RightStartLeftScore extends CommandGroup {
     
     public RightStartLeftScore() {
+    	
     	addSequential(new DriveGyroReset());
+    	addSequential(new DriveSpeedShift(true));
+    	addSequential(new DriveSpeedShift(true));
     	addSequential(new IntakePosition(true));
-    	addSequential(new DriveStraightBasic(-210, 60, true, true, 0));
+    	addSequential(new DriveStraightBasic(-215, 60, true, true, 0));
+    	addSequential(new ElevatorBasic(5));
     	addSequential(new DriveTurnBasic(true, 87, 300, MPSoftwareTurnType.TANK));
     	addSequential(new DriveStraightBasic(150, 60, true, true, 0));
+    	addSequential(new ElevatorBasic(30));
     	addSequential(new DriveTurnBasic(true, -87, 300, MPSoftwareTurnType.TANK));
-    	addSequential(new DriveStraightBasic(5, 60, true, true, 0));
+    	addSequential(new DriveStraightBasic(3, 60, true, true, 0));
     	addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
     	//addSequential(new WaitCommand(.1));
     	addSequential(new IntakePosition(false));
-    	addSequential(new WaitCommand(.5));
+    	addSequential(new WaitCommand(.2));
     	addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));
-
+    	addSequential(new DriveStraightBasic(-10, 60, true, true, 0));
     	//addSequential(new DriveStraightMP(60, 10, true, true, 0));  // 95 for 112" greenville
        
     }

src/org/usfirst/frc4388/robot/commands/auton/RightSwitchAuton.java

@@ -6,12 +6,12 @@ import org.usfirst.frc4388.robot.commands.DriveSpeedShift;
 import org.usfirst.frc4388.robot.commands.DriveStraightBasic;
 import org.usfirst.frc4388.robot.commands.DriveStraightMP;
 import org.usfirst.frc4388.robot.commands.DriveTurnBasic;
+import org.usfirst.frc4388.robot.commands.ElevatorBasic;
 import org.usfirst.frc4388.robot.commands.ElevatorSetSpeed;
 import org.usfirst.frc4388.robot.commands.IntakePosition;
 import org.usfirst.frc4388.robot.commands.IntakeSetSpeed;
 import org.usfirst.frc4388.robot.subsystems.Carriage;
 import org.usfirst.frc4388.robot.subsystems.Drive;
-import org.usfirst.frc4388.robot.subsystems.ElevatorAuton;
 import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
@@ -24,13 +24,13 @@ public class RightSwitchAuton extends CommandGroup {
     
     public RightSwitchAuton() {
     	addSequential(new DriveGyroReset());
+    	addSequential(new DriveSpeedShift(true));
     	addSequential(new IntakePosition(true));
-    	addSequential(new DriveStraightBasic(-150, 60, true, true, 0));
+    	addSequential(new DriveStraightBasic(-130, 60, true, true, 0));
-    	addSequential(new ElevatorSetSpeed(1.00));
+    	addSequential(new ElevatorBasic(30));
-    	addSequential(new WaitCommand(.5));
-    	addSequential(new ElevatorSetSpeed(0.00));
     	addSequential(new DriveTurnBasic(true, 87, 300, MPSoftwareTurnType.TANK));
-    	addSequential(new DriveStraightBasic(9, 60, true, true, 0));
+    	addSequential(new ElevatorBasic(30));
+    	addSequential(new DriveStraightBasic(20, 60, true, true, 0));
     	addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED));
     	//addSequential(new WaitCommand(.1));
     	addSequential(new IntakePosition(false));

src/org/usfirst/frc4388/robot/subsystems/Elevator.java

@@ -59,7 +59,7 @@ public class Elevator extends Subsystem implements ControlLoopable
 	private double targetPPosition;
 	
 	//Encoder ticks to inches for encoders
-	public static final double ENCODER_TICKS_TO_INCHES = Constants.kDriveEncoderTicksPerInch;
+	public static final double ENCODER_TICKS_TO_INCHES = Constants.kElevatorEncoderTicksPerInch;
 	
 	//control mode for joystick control
 	private DriveControlMode controlMode = DriveControlMode.JOYSTICK;
@@ -82,6 +82,7 @@ public class Elevator extends Subsystem implements ControlLoopable
 	public boolean pressed;
 	SensorCollection isPressed;
 	
+	public boolean elevatorControlMode = false;
 	// Motor controllers
 	//private ArrayList<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();	
     
@@ -93,10 +94,11 @@ public class Elevator extends Subsystem implements ControlLoopable
 			elevatorRight = new CANTalonEncoder(RobotMap.ELEVATOR_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder);
 			elevatorLeft = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR2_ID);
 			
-			//Setting right elevator motor as follower
+    		elevatorRight.setInverted(false);
+
+			//Setting left elevator motor as follower
     		elevatorLeft.set(ControlMode.Follower, elevatorRight.getDeviceID());
     		elevatorLeft.setInverted(false);
-    		elevatorRight.setInverted(false);
     		
     		//Limit Switch Left
     		elevatorLeft.overrideLimitSwitchesEnable(true);
@@ -192,7 +194,7 @@ public class Elevator extends Subsystem implements ControlLoopable
     public void moveElevatorXbox()
     {
     	double moveElevatorInput;
-    	double elevatorSafeZone = 1.2; 
+    	double elevatorSafeZone = -30; 
     	
     	double elevatorPos = getEncoderElevatorPosition();
     	
@@ -205,19 +207,23 @@ public class Elevator extends Subsystem implements ControlLoopable
     	
     	if(elevatorTuningPressed == true)
       	{     		
-     		elevatorRight.set(moveElevatorInput * (0.5));
+     		elevatorRight.set(moveElevatorInput * 0.5);
       	}
      	else if(elevatorTuningPressed == false)
      	{
-     		if(elevatorPos <= elevatorSafeZone /*&& elevatorPos >= 0*/)
+     		elevatorRight.set(moveElevatorInput);
+     	}
+     		
+     		/*
+     		if(elevatorPos <= elevatorSafeZone && elevatorPos >= 0)
      		{
-     			elevatorRight.set(moveElevatorInput * 0.65);
+     			elevatorRight.set(moveElevatorInput);
      		}
      		else if(elevatorPos > elevatorSafeZone)
      		{
-     			elevatorRight.set(moveElevatorInput);
+     			elevatorRight.set(moveElevatorInput * 0.65);
+     			
      			
-     			/*
      			if(holdButtonPressed == true)
              	{
              		elevatorRight.set(-0.43 * (0.2));
@@ -226,18 +232,20 @@ public class Elevator extends Subsystem implements ControlLoopable
              	{
              		elevatorRight.set(moveElevatorInput * 0.75);
              	}
-             	*/
+             	
      		}
-     		/*
+     		
      		else if(elevatorPos < 0)
      		{
              	elevatorRight.set(moveElevatorInput * 0.75);
      		}
+     		
      		*/
-      	}
+    	
-     	
+    	System.out.println(moveElevatorInput);
-     	System.out.println(elevatorPos);		//-6.9 to 1.9   total: 8.8 range
     }
+     	
+//     	System.out.println(elevatorPos);		//-6.9 to 1.9   total: 8.8 range
     
 	//PID encoder position
 	public double getEncoderElevatorPosition()
@@ -245,6 +253,11 @@ public class Elevator extends Subsystem implements ControlLoopable
 		return elevatorRight.getPositionWorld();
 	}
 	
+	public double getElevatorHeightInchesAboveFloor()
+	{
+		return elevatorRight.getPositionWorld();
+	}
+
 	public synchronized void setControlMode(DriveControlMode controlMode) 
 	{
  		this.controlMode = controlMode;
@@ -284,6 +297,10 @@ public class Elevator extends Subsystem implements ControlLoopable
 		Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_LOWEST);
 	}
 	*/
+	public void rawSetOutput(double output){
+		elevatorRight.set(/*ControlMode.PercentOutput,*/ output);
+	}
+	
 	public void holdInPos()
 	{
 		elevatorRight.set(-0.43 * 0.2);
@@ -301,23 +318,30 @@ public class Elevator extends Subsystem implements ControlLoopable
 		
 		if(isPressed.isFwdLimitSwitchClosed() == true)
 		{
-			Robot.elevator.setControlMode(DriveControlMode.STOP_MOTORS);
+			if (controlMode == DriveControlMode.JOYSTICK) {
+				Robot.elevator.setControlMode(DriveControlMode.STOP_MOTORS);	
+			}
 			pressed = true;
 		}
 		else
 		{
-			Robot.elevator.setControlMode(DriveControlMode.JOYSTICK);
+			if 	(controlMode == DriveControlMode.STOP_MOTORS){
+				{
+				Robot.elevator.setControlMode(DriveControlMode.JOYSTICK);
+				}
+			
 			pressed = false;
+			}
 		}
+
+	}
 		
 		//pressed = (isPressed.isFwdLimitSwitchClosed() == true) ? true : false;
-	}
 	
 	
 	
-    public double getelevatorPositionWorld() {
+
-    	return elevatorRight.getPositionWorld();
+
-    }
 	@Override
 	public void controlLoopUpdate() 
 	{
@@ -344,6 +368,12 @@ public class Elevator extends Subsystem implements ControlLoopable
 			{
 				isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition());
 			}
+			/*
+			else if(controlMode == DriveControlMode.RAW)
+			{
+				isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition());
+			}
+			*/
 		}
 	}
 
@@ -355,7 +385,7 @@ public class Elevator extends Subsystem implements ControlLoopable
     @Override
     public void periodic() 
     {
-    	isSwitchPressed();
+    //	isSwitchPressed();
     }
     
 	@Override
@@ -386,6 +416,8 @@ public class Elevator extends Subsystem implements ControlLoopable
 		{
 			try 
 			{
+				SmartDashboard.putNumber("Elevator Pos Ticks", elevatorRight.getSelectedSensorPosition(0));
+				SmartDashboard.putNumber("Elevator Pos Inches", getElevatorHeightInchesAboveFloor());
 				//SmartDashboard.putData(pressed);
 			}
 			catch (Exception e) 
@@ -394,9 +426,7 @@ public class Elevator extends Subsystem implements ControlLoopable
 			}
 		}
 	}
-	public void rawMoveSteer(double move, double steer) {
+
-		m_drive.arcadeDrive(move, steer, false);
-	}
 	
 }
 

src/org/usfirst/frc4388/robot/subsystems/ElevatorAuton.java

@@ -1,120 +0,0 @@
-
-package org.usfirst.frc4388.robot.subsystems;
-
-import org.usfirst.frc4388.robot.RobotMap;
-import org.usfirst.frc4388.robot.commands.*;
-import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;
-import org.usfirst.frc4388.utility.CANTalonEncoder;
-import org.usfirst.frc4388.utility.ControlLoopable;
-
-import edu.wpi.first.wpilibj.command.Subsystem;
-import org.usfirst.frc4388.robot.OI;
-import org.usfirst.frc4388.robot.Robot;
-
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
-import com.ctre.phoenix.motorcontrol.ControlMode;
-import com.ctre.phoenix.motorcontrol.FeedbackDevice;
-import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
-import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
-import com.ctre.phoenix.motorcontrol.SensorCollection;
-
-
-
-/**
- *
- */
-public class ElevatorAuton extends Subsystem {
-
-	private WPI_TalonSRX ElevatorRight;
-	private WPI_TalonSRX ElevatorLeft;
-	public static final double RAISE_ELEVATOR_SPEED = 1;
-	public static final double LOWER_ELEVATOR_SPEED = -1;
-	public static final double STOP_ELEVATOR_SPEED = 0;
-	LimitSwitchSource limitSwitchSource;
-	public boolean pressed;
-	SensorCollection isPressed;
-	/////^^^^^^^^^ replace this line with the modes we need
-	
-	
-	private boolean isFinished;
-	//private CarriageControlMode controlMode = CarriageControlMode.JOYSTICK;
-	private double PeriodMs;
-	
-	
-	
-	
-	public ElevatorAuton() {
-		try {
-			ElevatorLeft = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR1_ID);
-			ElevatorRight = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR2_ID);
-			ElevatorRight.setInverted(false);
-			ElevatorLeft.setInverted(false);
-			
-			
-		
-			
-			//carriageLeft.set(CurrentLimit, value);
-			
-			ElevatorRight.set(ControlMode.Follower, ElevatorLeft.getDeviceID());
-		}
-		catch (Exception e) {
-			System.err.println("An error occurred in the elevator constructor");
-			
-			
-		}
-    }
-	
-	public void setRaiseSpeed(double raiseSpeed) {
-			ElevatorLeft.set(-raiseSpeed);
-    }
-		
-      //  public synchronized void setControlMode(CarriageControlMode controlMode) {
-     	//	this.controlMode = controlMode;
-    	//	if (controlMode == CarriageControlMode.JOYSTICK) {
-    			
-    		//	carriageLeft.set(ControlMode.PercentOutput, 0);	//TODO URGENT: make sure not called when robot moving
-    		//	carriageRight.set(ControlMode.PercentOutput, 0);	
-    	//	}
- //   }
-
-    	
-
-		@Override
-		/*public void controlLoopUpdate() {
-			pressed = false;
-    		isPressed = ElevatorLeft.getSensorCollection();
-			
-			elevator_input = Robot.oi.getOperatorJoystick().getRawAxis(RobotMap.OPERATOR_JOYSTICK_1_USB_ID);
-			System.err.println(isPressed.isFwdLimitSwitchClosed());
-			if (isPressed.isFwdLimitSwitchClosed() == true) {
-				ElevatorLeft.set(0);
-				ElevatorRight.set(0);
-				pressed = true;
-			} else {
-
-				ElevatorLeft.set(elevator_input);
-				ElevatorRight.set(elevator_input);
-				pressed = false;
-			}
-		}
-		*/
-
-	    public void periodic() {
-	   
-	    }
-
-
-		@Override
-	//	public void setPeriodMs(long periodMs) {
-		//	this.PeriodMs = periodMs;
-	//	}
-    	public void initDefaultCommand() {
-    	}
-    	public void isFinished() {
-    	}
-    	
-    	
-
-		
-}
-