fixed issues

addeed more wait time before spitting out cube.
crossbaseline now uses splines.
change comp bot robot width
changed center right and left endpoiints
updated mottion profilling

PowerUp/src/org/usfirst/frc/team2974/robot/Config.java

@@ -14,7 +14,7 @@ private Config() {
 	}
 
 	public enum Robot {
-		PRACTICE(0.7800, 0.000409, false), COMPETITION(0.7239, 0.0002045, true);
+		PRACTICE(0.7800, 0.000409, false), COMPETITION(0.7800, 0.0002045, true);
 
 
 		private final double robotWidth;
@@ -105,7 +105,7 @@ private Input() {
 	public static final class Elevator {
 
 		public static final double TOLERANCE = 0.1;
-		public static final double INCHES_TO_NU = 775; // TODO: FIXME
+		public static final double INCHES_TO_NU = 775; // TODO: improve number to improve accuracy
 
 		public static final double HIGH_HEIGHT = 72; // inches, this gets the scale
 		public static final double MEDIUM_HEIGHT = 24.00; // inches, this gets the switch, exchange top, and portal
@@ -116,7 +116,7 @@ public static final class Elevator {
 
 		public static final double NUDGE_DISTANCE = 1; // in inches
 
-		public static final int CRUISE_SPEED = 2000; // native sensor units per 100 ms
+		public static final int CRUISE_SPEED = 2500; // native sensor units per 100 ms
 		public static final int ACCELERATION = 2000; // ^^^ per second
 
 		// motion control constants
@@ -162,10 +162,8 @@ public static final class Path {
 		public static final Pose R10 = new Pose(0.83147, 5.80987, 0);
 
 		public static final Pose C0 = new Pose(0.19177, 0.42835, 90);
-		public static final Pose C1 = new Pose(1.29388/*testing values 1.29388*/,
-			3.22791/*testing values*/  /*previous 3.12791*/, 90);
-		public static final Pose C2 = new Pose(-1.29388/*testing values -1.29388*/,
-			3.22791 /*testing values*/  /*previous 3.12791 */, 90);
+		public static final Pose C1 = new Pose(1.59388, 3.12791, 90);
+		public static final Pose C2 = new Pose(-1.59388, 3.12791, 90);
 
 		// we can do this because every point is measured from the center line.
 		public static final Pose L0 = negateX(R0);

PowerUp/src/org/usfirst/frc/team2974/robot/Robot.java

@@ -10,13 +10,11 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import org.usfirst.frc.team2974.robot.command.auton.GamePosition;
-import org.usfirst.frc.team2974.robot.command.auton.SimpleSpline;
 import org.usfirst.frc.team2974.robot.subsystems.Drivetrain;
 import org.usfirst.frc.team2974.robot.subsystems.Elevator;
 import org.usfirst.frc.team2974.robot.subsystems.IntakeOutput;
 import org.usfirst.frc.team2974.robot.util.ElevatorLogger;
 import org.waltonrobotics.MotionLogger;
-import org.waltonrobotics.controller.Pose;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -35,7 +33,6 @@ public class Robot extends IterativeRobot {
 	private static String gameData; // for ease of access
 	private CommandGroup autonCommands;
 	//    private static SendableChooser<Config.Robot> robotChooser = new SendableChooser<>();
-	private SendableChooser<Boolean> doNothingChooser;
 	private SendableChooser<Character> startLocation;
 
 	public static Config.Robot getChoosenRobot() {
@@ -71,11 +68,8 @@ public void robotInit() {
 		intakeOutput = new IntakeOutput();
 		elevator = new Elevator(elevatorLogger);
 
-		doNothingChooser = new SendableChooser<>();
-		doNothingChooser.addObject("Do Nothing!", true);
-		doNothingChooser.addDefault("Please move!", false);
-
 		startLocation = new SendableChooser<>();
+		startLocation.addObject("Do Nothing", ' ');
 		startLocation.addObject("Left", 'l');
 		startLocation.addObject("Right", 'r');
 		startLocation.addDefault("Center", 'c');
@@ -84,8 +78,8 @@ public void robotInit() {
 //        robotChooser.addDefault("Competition", Config.Robot.COMPETITION);
 
 //        SmartDashboard.putData("TEST AUTON", SimpleSpline.pathFromPosesWithAngle(false, new Pose(0, 0, 90), new Pose(0, 1, 90), new Pose(1, 2, 0), new Pose(2, 2, 0)));
-		SmartDashboard.putData("6m drive straight",
-			SimpleSpline.pathFromPosesWithAngle(false, new Pose(0, 0, 90), new Pose(0, 6, 90)));
+//		SmartDashboard.putData("6m drive straight",
+//			SimpleSpline.pathFromPosesWithAngle(false, new Pose(0, 0, 90), new Pose(0, 6, 90)));
 
 		updateSmartDashboard();
 	}
@@ -96,7 +90,7 @@ public void disabledInit() {
 		gameData = null;
 		drivetrain.reset();
 		motionLogger.writeMotionDataCSV();
-		elevatorLogger.writeMotionDataCSV();
+//		elevatorLogger.writeMotionDataCSV();
 	}
 
 	@Override
@@ -107,25 +101,35 @@ public void disabledPeriodic() {
 
 	@Override
 	public void autonomousInit() {
+//		drivetrain.cancelControllerMotion();
 		drivetrain.startControllerMotion();
 		elevator.startZero();
 		motionLogger.initialize();
 		elevatorLogger.initialize();
 //        drivetrain.shiftDown();
 		drivetrain.shiftUp();
 
-		if (doNothingChooser.getSelected()) { // if should do nothing
-			System.out.println(">:( Nothing has been chosen. Scrubs.");
-			autonCommands = GamePosition.DO_NOTHING.getCommand();
-			return; // skips the rest of the init! WARNING: PUT NEEDED CODE BEFORE THIS!
-		}
+//		if (doNothingChooser.getSelected()) { // if should do nothing
+//			System.out.println(">:( Nothing has been chosen. Scrubs.");
+//			autonCommands = GamePosition.DO_NOTHING.getCommand();
+//			return; // skips the rest of the init! WARNING: PUT NEEDED CODE BEFORE THIS!
+//		}
 
 		while ((gameData == null) || gameData.isEmpty()) {
 			gameData = DriverStation.getInstance().getGameSpecificMessage(); // "LRL" or something
 		}
 
 		char startPosition = startLocation.getSelected();
 
+		if (startPosition == ' ') { // if should do nothing
+			System.out.println(">:( Nothing has been chosen. Scrubs.");
+			autonCommands = GamePosition.DO_NOTHING.getCommand();
+			return; // skips the rest of the init! WARNING: PUT NEEDED CODE BEFORE THIS!
+		}
+
+		//TODO remove this when we have the elevator able to reach the scale
+		gameData = gameData.substring(0, 1) + "..";
+
 		System.out.println(startPosition);
 		System.out.println(gameData);
 		System.out.println(GamePosition.getGamePosition(startPosition, gameData));
@@ -155,7 +159,7 @@ public void teleopInit() {
 		}
 
 		drivetrain.cancelControllerMotion();
-		elevator.enableControl();
+		elevator.disableControl();
 		drivetrain.shiftUp(); // start in high gear
 		drivetrain.reset();
 	}
@@ -188,8 +192,7 @@ private void updateSmartDashboard() {
 		SmartDashboard.putNumber("Right", RobotMap.encoderRight.getDistance());
 
 		// Selectors
-		SmartDashboard.putData("Start Location", startLocation);
-		SmartDashboard.putData("Do Nothing", doNothingChooser);
+		SmartDashboard.putData("Drive Team/Start Location", startLocation);
 
 		// Elevator
 		SmartDashboard.putNumber("Elevator Position (inches)", elevator.getCurrentPosition());

PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/CrossBaseline.java

@@ -8,6 +8,7 @@
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 import org.usfirst.frc.team2974.robot.Robot;
+import org.waltonrobotics.controller.Pose;
 
 /**
  * Command for crossing the baseline. Usage is: new CrossBaseline().left();
@@ -45,7 +46,10 @@ public CrossBaseline right(double yMovement) {
 		isOptionSelected = true;
 
 		// drive forward x meters
-		addSequential(new DriveStraightByDistance(VELOCITY_MAX, ACCELERATION_MAX, yMovement));
+
+		addSequential(SimpleSpline
+			.pathFromPosesWithAngle(VELOCITY_MAX, ACCELERATION_MAX, false, new Pose(0, 0, 90),
+				new Pose(0, yMovement, 90)));
 		return this;
 	}
 

PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/DriveSwitchScale.java

@@ -31,7 +31,7 @@ public DriveSwitchScale left() {
 		addSequential(SimpleSpline.pathFromPosesWithAngle(true, L5, L7, L8));
 		addParallel(new ElevatorTarget(LOW_HEIGHT));
 		addSequential(SimpleSpline.pathFromPosesWithAngle(false, L8, L9));
-		addSequential(new FindCube());
+		addSequential(new FindCube().left());
 		// TODO: gather cube too
 		addSequential(SimpleSpline.pathFromPosesWithAngle(true, L9, L10));
 		addParallel(new ElevatorTarget(HIGH_HEIGHT));
@@ -54,7 +54,7 @@ public DriveSwitchScale right() {
 		addSequential(SimpleSpline.pathFromPosesWithAngle(true, R5, R7, R8));
 		addParallel(new ElevatorTarget(LOW_HEIGHT));
 		addSequential(SimpleSpline.pathFromPosesWithAngle(false, R8, R9));
-		addSequential(new FindCube());
+		addSequential(new FindCube().right());
 		addSequential(SimpleSpline.pathFromPosesWithAngle(true, R9, R10));
 		addParallel(new ElevatorTarget(HIGH_HEIGHT));
 		addSequential(SimpleSpline.pathFromPosesWithAngle(false, R10, R6, R2, R3));

PowerUp/src/org/usfirst/frc/team2974/robot/command/auton/FindCube.java

@@ -1,17 +1,22 @@
 package org.usfirst.frc.team2974.robot.command.auton;
 
-import edu.wpi.first.wpilibj.command.Command;
-
 /**
  * Finds the power cube and positions itself so it is in front of it.
  *
  * TODO: make the class function
  */
-public class FindCube extends Command {
+public class FindCube extends AutonOption {
 
-	@Override
-	protected boolean isFinished() {
-		return true;
+	public FindCube right() {
+		return this;
+	}
+
+	public FindCube left() {
+		return this;
 	}
 
+	@Override
+	public AutonOption center() {
+		return this;
+	}
 }

PowerUp/src/org/usfirst/frc/team2974/robot/subsystems/Drivetrain.java

@@ -28,7 +28,7 @@ public Drivetrain(MotionLogger motionLogger) {
 		driveMode = new SendableChooser<>();
 		driveMode.addDefault("Tank", true);
 		driveMode.addObject("Cheesy", false);
-		SmartDashboard.putData("Drive Mode", driveMode);
+		SmartDashboard.putData("Drive Team/Drive Mode", driveMode);
 
 		motorRight.setInverted(true);
 

PowerUp/src/org/usfirst/frc/team2974/robot/util/AutonUtil.java

@@ -16,7 +16,7 @@ public static <T extends AutonOption> T driveToSinglePoint(T auton, double eleva
 		Pose... splinePoints) {
 		auton.addParallel(new ElevatorTarget(elevatorHeight));
 		auton.addSequential(SimpleSpline.pathFromPosesWithAngle(false, splinePoints));
-		auton.addSequential(new WaitCommand(0.5));
+		auton.addSequential(new WaitCommand(1));
 		auton.addSequential(new DropCube());
 
 		auton.setOptionSelected(true);