Merge pull request #109 from DriverStationComputer/better-auto

lhmcgann · web-flow · commit 4e0dad25d1d4 · 2018-05-02T19:33:40.000-07:00
Better auto; last good competition changes
diff --git a/AAAScripts/scripts.txt b/AAAScripts/scripts.txt
@@ -7,9 +7,11 @@ moveto (0,120) #Drive past auto line
 
 #Switch
 RRxx:
-moveto (0,155) 
-turn -90
-switch
+moveto (0,148) -90
+switch 18
+moveto (-10, 220)
+intake
+moveto (-22, 205)
 
 RLxx:
 move 56
@@ -76,9 +78,11 @@ moveto (0, 120) #Drive past auto line
 
 #Switch
 LLxx:
-move 150
-turn 90
-switch
+moveto (0,148) 90
+switch 18
+moveto (10, 220)
+intake
+moveto (22, 205)
 
 LRxx:
 move 56
@@ -139,17 +143,22 @@ exchange
 
 # Cross Auto Line 
 Cxxx: 
-moveto (0,50) (48,50) (48,92)  #cross baseline
-
+moveto (0, 12) (40, 88)
 
 # Switch
 CRxx:
-moveto (0,50) (48,50) (48,85) 
-switch #deploy switch 
+moveto (0,15) (50,80) 0
+switch 16
+move -40
+intake
+moveto (20, 65)
 
 CLxx:
-moveto (0,50) (-56,50) (-56,85)
-switch #deploy switch 
+moveto (0,15) (-60,80) 0
+switch 16
+move -40
+intake
+moveto (-30, 65)
 
 
 # Switch and Exchange
diff --git a/CompSD.xml b/CompSD.xml
@@ -1,12 +1,25 @@
 <xml version="1.0">
 <dashboard>
+	<static-widget class="edu.wpi.first.smartdashboard.gui.elements.CameraServerViewer">
+		<location x="260" y="17"/>
+		<width>552</width>
+		<height>403</height>
+		<property name="Degrees Rotation" value="180"/>
+		<property name="Camera Choice" value=""/>
+	</static-widget>
+	<widget field="Right Intake Open" type="Boolean" class="edu.wpi.first.smartdashboard.gui.elements.TextBox">
+		<location x="37" y="250"/>
+	</widget>
+	<widget field="Left Intake Open" type="Boolean" class="edu.wpi.first.smartdashboard.gui.elements.TextBox">
+		<location x="37" y="229"/>
+	</widget>
 	<widget field="DT is Inverted" type="Boolean" class="edu.wpi.first.smartdashboard.gui.elements.BooleanBox">
 		<location x="11" y="110"/>
 		<width>118</width>
 		<height>59</height>
 	</widget>
 	<widget field="Has Cube" type="Boolean" class="edu.wpi.first.smartdashboard.gui.elements.BooleanBox">
-		<location x="697" y="463"/>
+		<location x="1182" y="402"/>
 		<width>96</width>
 		<height>30</height>
 	</widget>
@@ -15,19 +28,6 @@
 		<width>113</width>
 		<height>44</height>
 	</widget>
-	<static-widget class="edu.wpi.first.smartdashboard.gui.elements.CameraServerViewer">
-		<location x="783" y="0"/>
-		<width>571</width>
-		<height>448</height>
-		<property name="Camera Choice" value="USB Camera 1"/>
-	</static-widget>
-	<static-widget class="edu.wpi.first.smartdashboard.gui.elements.CameraServerViewer">
-		<location x="191" y="7"/>
-		<width>586</width>
-		<height>438</height>
-		<property name="Degrees Rotation" value="180"/>
-		<property name="Camera Choice" value=""/>
-	</static-widget>
 	<widget field="Move PID Error" type="Number" class="edu.wpi.first.smartdashboard.gui.elements.TextBox">
 		<location x="173" y="661"/>
 	</widget>
@@ -129,13 +129,13 @@
 </dashboard>
 <live-window>
 	<widget field="Drivetrain" type="LW Subsystem" class="edu.wpi.first.smartdashboard.livewindow.elements.LWSubsystem">
-		<location x="7" y="6"/>
+		<location x="3" y="14"/>
 	</widget>
 	<widget field="ClimberAssist" type="LW Subsystem" class="edu.wpi.first.smartdashboard.livewindow.elements.LWSubsystem">
-		<location x="21" y="22"/>
+		<location x="9" y="11"/>
 	</widget>
 	<widget field="IntakeEject" type="LW Subsystem" class="edu.wpi.first.smartdashboard.livewindow.elements.LWSubsystem">
-		<location x="18" y="28"/>
+		<location x="30" y="22"/>
 	</widget>
 	<widget field="Lift" type="LW Subsystem" class="edu.wpi.first.smartdashboard.livewindow.elements.LWSubsystem">
 		<widget field="PIDController" type="PIDController" class="edu.wpi.first.smartdashboard.gui.elements.PIDEditor">
@@ -153,12 +153,12 @@
 			<height>46</height>
 			<width>293</width>
 		</widget>
-		<location x="24" y="18"/>
+		<location x="31" y="11"/>
 		<width>305</width>
 		<height>323</height>
 	</widget>
 	<widget field="Climber" type="LW Subsystem" class="edu.wpi.first.smartdashboard.livewindow.elements.LWSubsystem">
-		<location x="5" y="16"/>
+		<location x="23" y="3"/>
 	</widget>
 	<widget field="Ungrouped" type="LW Subsystem" class="edu.wpi.first.smartdashboard.livewindow.elements.LWSubsystem">
 		<widget field="Encoder[1]" type="Quadrature Encoder" class="edu.wpi.first.smartdashboard.livewindow.elements.EncoderDisplay">
@@ -251,7 +251,7 @@
 			<height>20</height>
 			<width>326</width>
 		</widget>
-		<location x="6" y="24"/>
+		<location x="8" y="22"/>
 		<width>338</width>
 		<height>974</height>
 	</widget>
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java
@@ -18,7 +18,6 @@
 import org.usfirst.frc.team199.Robot2018.commands.PIDTurn;
 import org.usfirst.frc.team199.Robot2018.commands.ResetEncoders;
 import org.usfirst.frc.team199.Robot2018.commands.SetDistancePerPulse;
-import org.usfirst.frc.team199.Robot2018.commands.ShiftDriveType;
 import org.usfirst.frc.team199.Robot2018.commands.ShiftHighGear;
 import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear;
 import org.usfirst.frc.team199.Robot2018.commands.StopIntake;
@@ -44,7 +43,7 @@ public class OI {
 	public Joystick rightJoy;
 	private JoystickButton shiftLowGearButton;
 	private JoystickButton shiftHighGearButton;
-	private JoystickButton shiftDriveTypeButton;
+	// private JoystickButton shiftDriveTypeButton;
 	private JoystickButton pIDMoveButton;
 	private JoystickButton pIDTurnButton;
 	private JoystickButton resetEncButton;
@@ -82,8 +81,9 @@ public int getButton(String key, int def) {
 
 	public OI(Robot robot) {
 		leftJoy = new Joystick(0);
-		shiftDriveTypeButton = new JoystickButton(leftJoy, getButton("Shift Drive Type", 2));
-		shiftDriveTypeButton.whenPressed(new ShiftDriveType());
+		// shiftDriveTypeButton = new JoystickButton(leftJoy, getButton("Shift Drive
+		// Type", 2));
+		// shiftDriveTypeButton.whenPressed(new ShiftDriveType());
 
 		invertDTButton = new JoystickButton(leftJoy, getButton("Invert Drivetrain", 3));
 		invertDTButton.whenPressed(new InvertDrivetrain());
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
@@ -183,8 +183,8 @@ public void robotInit() {
 
 		listen = new Listener();
 		lift.resetEnc();
-		CameraServer.getInstance().startAutomaticCapture(0);
-		CameraServer.getInstance().startAutomaticCapture(1);
+		// CameraServer.getInstance().startAutomaticCapture(0);
+		CameraServer.getInstance().startAutomaticCapture((int) Robot.getConst("Camera Port", 1));
 	}
 
 	/**
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java
@@ -109,21 +109,21 @@ public static boolean isValidCommand(String instruction, String args, int lineNu
 		// moveto takes in a set of points, and the last arg can be a number
 		if (instruction.equals("moveto")) {
 			if (args == "") {
-				logError(lineNumber, "The command `moveto` requires at least one argument.");
+				logError(lineNumber, "The command `" + instruction + "` requires at least one argument.");
 				return false;
 			}
 
 			String[] splitArgs = args.split(" ");
 			for (int i = 0; i < splitArgs.length - 1; i++) {
 				if (!isPoint(splitArgs[i])) {
 					logError(lineNumber,
-							"The arguments for command `moveto` should be points formatted like this: " + "`(x,y)`.");
+							"The arguments for command `" + instruction + "` should be points formatted like this: " + "`(x,y)`.");
 					return false;
 				}
 			}
 
 			if (!isDouble(splitArgs[splitArgs.length - 1]) && !isPoint(splitArgs[splitArgs.length - 1])) {
-				logError(lineNumber, "The last argument for command `moveto` should be a number, or a point "
+				logError(lineNumber, "The last argument for command `" + instruction + "` should be a number, or a point "
 						+ "formatted like this: `(x,y)`.");
 				return false;
 			}
@@ -132,32 +132,32 @@ public static boolean isValidCommand(String instruction, String args, int lineNu
 		// turn can take a number or point
 		else if (instruction.equals("turn")) {
 			if (args.contains(" ")) {
-				logError(lineNumber, "Command `turn` only accepts one argument.");
+				logError(lineNumber, "Command `" + instruction + "` only accepts one argument.");
 				return false;
 			}
 
 			if (!isDouble(args) && !isPoint(args)) {
-				logError(lineNumber, "The argument for command `turn` should be a number or a point formatted like "
+				logError(lineNumber, "The argument for command `" + instruction + "` should be a number or a point formatted like "
 						+ "this: `(x,y)`.");
 				return false;
 			}
 		}
 
-		// move and wait can take only a number
-		else if (instruction.equals("move") || instruction.equals("wait")) {
+		// these can take only a number
+		else if (instruction.equals("move") || instruction.equals("wait") || instruction.equals("switch") || instruction.equals("scale")) {
 			if (args.contains(" ")) {
-				logError(lineNumber, "Command `move` only accepts one argument.");
+				logError(lineNumber, "Command `" + instruction + "` only accepts one argument.");
 				return false;
 			}
 
 			if (!isDouble(args)) {
-				logError(lineNumber, "The argument for command `move` should be a number.");
+				logError(lineNumber, "The argument for command `" + instruction + "` should be a number.");
 				return false;
 			}
 		}
 
 		// switch, scale, exchange, intake, and end all don't have any args
-		else if (instruction.equals("switch") || instruction.equals("scale") || instruction.equals("exchange")
+		else if (instruction.equals("exchange")
 				|| instruction.equals("intake") || instruction.equals("end")) {
 			if (!args.equals("")) {
 				logError(lineNumber, "Command `" + instruction + "` does not accept any arguments.");
@@ -168,7 +168,7 @@ else if (instruction.equals("switch") || instruction.equals("scale") || instruct
 		// Jump only takes one argument
 		else if (instruction.equals("jump")) {
 			if (args.contains(" ")) {
-				logError(lineNumber, "Command `jump` only accepts one argument.");
+				logError(lineNumber, "Command `" + instruction + "` only accepts one argument.");
 				return false;
 			}
 		}
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/aaa-reference.md b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/aaa-reference.md
@@ -50,7 +50,7 @@ Ex:
 | jump | Jumps to the specified script and continues the current script when finished. (Doesn’t make the robot go up.) | `jump MoveToRScale`
 | move | Move forward or backwards for the specified amount in inches, relative to the current position. | `move 24` |
 | moveto | Move to 1 or more points, sequentially, with an optional last value having a final angle to face towards, all relative to the starting position. | `moveto (12,0) (36,12)` <br> `moveto (0,12) 90` |
-| scale | Place a cube at scale height. | `scale` |
-| switch | Place a cube at switch height. | `switch` |
+| scale | Place a cube at scale height after moving forward by the specified amount with the lift up, and after moves 12 inches back. | `scale 24` |
+| switch | Place a cube at switch height after moving forward by the specified amount with the lift up, and after moves 12 inches back. | `switch 24` |
 | turn | Turn towards a relative point or rotate clockwise by an angle in degrees, relative to the current position. <br> Negative angle for counterclockwise. | `turn (36,48)` <br> `turn 45` |
 | wait | Waits for the number of seconds before proceeding to next command. | `wait 5` |
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoLift.java
@@ -13,9 +13,7 @@ public class AutoLift extends CommandGroup {
 	public AutoLift(Lift lift, String height) {
 		if (height.equals("GROUND")) {
 			addSequential(new LiftToPosition(lift, LiftHeight.toLH("HOLD_CUBE")));
-			addSequential(new LiftToPosition(lift, LiftHeight.toLH(height)));
-		} else {
-			addSequential(new LiftToPosition(lift, LiftHeight.toLH(height)));
 		}
+		addSequential(new LiftToPosition(lift, LiftHeight.toLH(height)));
 	}
 }
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToScale.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToScale.java
@@ -9,12 +9,11 @@
  */
 public class EjectToScale extends CommandGroup {
 
-	public EjectToScale() {
+	public EjectToScale(double dist) {
 		addSequential(new AutoLift(Robot.lift, "SCALE"));
-		addSequential(
-				new PIDMove(Robot.getConst("Auto Scale Move Dist", 18), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
+		addSequential(new PIDMove(dist, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
 		addSequential(new OuttakeCube());
-		addSequential(new PIDMove(-1 * Robot.getConst("Auto Scale Move Dist", 18), Robot.dt, Robot.sd,
+		addSequential(new PIDMove(-1 * Robot.getConst("Scale move back dist", 12), Robot.dt, Robot.sd,
 				Robot.dt.getDistEncAvg()));
 		addSequential(new AutoLift(Robot.lift, "GROUND"));
 	}
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToSwitch.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/EjectToSwitch.java
@@ -9,12 +9,11 @@
  */
 public class EjectToSwitch extends CommandGroup {
 
-	public EjectToSwitch() {
+	public EjectToSwitch(double dist) {
 		addSequential(new AutoLift(Robot.lift, "SWITCH"));
-		addSequential(
-				new PIDMove(Robot.getConst("Auto Switch Move Dist", 18), Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
+		addSequential(new PIDMove(dist, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
 		addSequential(new OuttakeCube());
-		addSequential(new PIDMove(-1 * Robot.getConst("Auto Switch Move Dist", 18), Robot.dt, Robot.sd,
+		addSequential(new PIDMove(-1 * Robot.getConst("Switch move back dist", 12), Robot.dt, Robot.sd,
 				Robot.dt.getDistEncAvg()));
 		addSequential(new AutoLift(Robot.lift, "GROUND"));
 	}
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LiftToPosition.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/LiftToPosition.java
@@ -24,8 +24,9 @@ public LiftToPosition(Lift lift, LiftHeight goal) {
 	// Called just before this Command runs the first time
 	@Override
 	protected void initialize() {
-		lift.getPIDController().setPID(Robot.getConst("LiftkP", 0.1), Robot.getConst("LiftkI", 0),
-				Robot.getConst("LiftkD", 0), Robot.getConst("LiftkF", 0.1));
+		// lift.getPIDController().setPID(Robot.getConst("LiftkP", 0.1),
+		// Robot.getConst("LiftkI", 0),
+		// Robot.getConst("LiftkD", 0), Robot.getConst("LiftkF", 0.1));
 		double setpoint = lift.getDesiredDistFromPos(pos);
 		lift.setSetpoint(setpoint);
 		System.out.println("Target Height: " + setpoint);
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunScript.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunScript.java
@@ -44,10 +44,10 @@ public RunScript(String scriptName) {
 				addSequential(new PIDMove(distance, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()));
 				break;
 			case "switch":
-				addSequential(new EjectToSwitch());
+				addSequential(new EjectToSwitch(Double.parseDouble(cmdArgs)));
 				break;
 			case "scale":
-				addSequential(new EjectToScale());
+				addSequential(new EjectToScale(Double.parseDouble(cmdArgs)));
 				break;
 			case "exchange":
 				addSequential(new EjectToExchange());
@@ -56,7 +56,7 @@ public RunScript(String scriptName) {
 				addSequential(new WaitCommand(Double.parseDouble(cmdArgs)));
 				break;
 			case "intake":
-				addSequential(new IntakeCube());
+				addParallel(new IntakeCube());
 				break;
 			case "jump":
 				addSequential(new RunScript(cmdArgs));
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/UpdateLiftPosition.java
@@ -54,7 +54,7 @@ protected void execute() {
 			goToGround = false;
 		}
 
-		if (goToGround || angle != -1) {
+		if (/* goToGround || */ angle != -1) {
 			desiredDist = lift.getDesiredDistFromPos(desiredPos);
 			lift.setSetpoint(desiredDist);
 		}
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java
@@ -127,6 +127,8 @@ public void runIntake(double speed) {
 	public void toggleIntake() {
 		toggleLeftIntake();
 		toggleRightIntake();
+		SmartDashboard.putBoolean("Left Intake Open", leftOpen);
+		SmartDashboard.putBoolean("Right Intake Open", rightOpen);
 	}
 
 	/**
diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Lift.java
@@ -61,7 +61,7 @@ public Lift() {
 		WIGGLE_ROOM = (int) Robot.getConst("Lift wiggle room", 3.0); // inches
 
 		// calculate constant measurements
-		GROUND_DIST = 0;
+		GROUND_DIST = 0.7;
 		HOLD_CUBE_DIST = 4;
 		// distance to switch 18.75 inches in starting position
 		SWITCH_DIST = (18.75 + WIGGLE_ROOM) / NUM_STAGES;

Original file line number	Diff line number	Diff line change
`@@ -183,8 +183,8 @@ public void robotInit() {`
`183`	`183`
`184`	`184`	`listen = new Listener();`
`185`	`185`	`lift.resetEnc();`
`186`		`- CameraServer.getInstance().startAutomaticCapture(0);`
`187`		`- CameraServer.getInstance().startAutomaticCapture(1);`
	`186`	`+ // CameraServer.getInstance().startAutomaticCapture(0);`
	`187`	`+ CameraServer.getInstance().startAutomaticCapture((int) Robot.getConst("Camera Port", 1));`
`188`	`188`	`}`
`189`	`189`
`190`	`190`	`/**`
Original file line number	Diff line number	Diff line change
`@@ -13,9 +13,7 @@ public class AutoLift extends CommandGroup {`
`13`	`13`	`public AutoLift(Lift lift, String height) {`
`14`	`14`	`if (height.equals("GROUND")) {`
`15`	`15`	`addSequential(new LiftToPosition(lift, LiftHeight.toLH("HOLD_CUBE")));`
`16`		`- addSequential(new LiftToPosition(lift, LiftHeight.toLH(height)));`
`17`		`- } else {`
`18`		`- addSequential(new LiftToPosition(lift, LiftHeight.toLH(height)));`
`19`	`16`	`}`
	`17`	`+ addSequential(new LiftToPosition(lift, LiftHeight.toLH(height)));`
`20`	`18`	`}`
`21`	`19`	`}`