Add clamp to move speed when during auto and intake is up

AAAScripts/scripts.txt

@@ -9,15 +9,14 @@ moveto (0,120) #Drive past auto line
 RRxx:
 moveto (0,155) 
 turn -90
-move 10
 switch
 
 RLxx:
 move 56
 turn -90
 move 170
 turn 90
-move 35
+move 25
 switch
 
 
@@ -35,7 +34,6 @@ exchange
 RRxE:
 moveto (0,155) 
 turn -90
-move 12
 switch
 turn -90
 move 105
@@ -55,7 +53,7 @@ move 56
 turn -90
 move 170
 turn 90
-move 35
+move 25
 switch
 move -40
 turn 90
@@ -80,15 +78,14 @@ moveto (0, 120) #Drive past auto line
 LLxx:
 move 150
 turn 90
-move 12
 switch
 
 LRxx:
 move 56
 turn 90
 move 170
 turn -90
-move 35
+move 25
 switch	
 
 
@@ -106,7 +103,6 @@ exchange
 LLxE:
 move 150
 turn 90
-move 12
 switch
 turn 90
 move 95
@@ -126,7 +122,7 @@ move 56
 turn 90
 move 170
 turn -90
-move 35
+move 25
 switch
 move -40
 turn -90
@@ -148,17 +144,17 @@ moveto (0,50) (48,50) (48,92)  #cross baseline
 
 # Switch
 CRxx:
-moveto (0,50) (48,50) (48,95) 
+moveto (0,50) (48,50) (48,85) 
 switch #deploy switch 
 
 CLxx:
-moveto (0,50) (-56,50) (-56,95)
+moveto (0,50) (-56,50) (-56,85)
 switch #deploy switch 
 
 
 # Switch and Exchange
 CRxE:
-moveto (0,50) (48,50) (48,95) 
+moveto (0,50) (48,50) (48,85) 
 switch #deploy switch 
 move -48 # 3 feet back
 turn -90
@@ -173,7 +169,7 @@ exchange
 
 
 CLxE:
-moveto (0,50) (-56,50) (-56,95) 
+moveto (0,50) (-56,50) (-56,85) 
 switch #deploy switch 
 move -48 # 3 feet back
 turn 90

CompSD.xml

@@ -0,0 +1,82 @@
+<xml version="1.0">
+<dashboard>
+	<widget field="Has Cube" type="Boolean" class="edu.wpi.first.smartdashboard.gui.elements.TextBox">
+		<location x="0" y="132"/>
+	</widget>
+	<widget field="Intake Current Right" type="Number" class="edu.wpi.first.smartdashboard.gui.elements.TextBox">
+		<location x="0" y="111"/>
+	</widget>
+	<widget field="Intake Current Left" type="Number" class="edu.wpi.first.smartdashboard.gui.elements.TextBox">
+		<location x="0" y="90"/>
+	</widget>
+	<widget field="Left Current draw" type="Number" class="edu.wpi.first.smartdashboard.gui.elements.TextBox">
+		<location x="482" y="581"/>
+	</widget>
+	<static-widget class="edu.wpi.first.smartdashboard.gui.elements.RobotPreferences">
+		<location x="875" y="450"/>
+	</static-widget>
+	<widget field="Intake Open" type="Boolean" class="edu.wpi.first.smartdashboard.gui.elements.TextBox">
+		<location x="487" y="556"/>
+	</widget>
+	<widget field="Right Solenoid Open" type="Boolean" class="edu.wpi.first.smartdashboard.gui.elements.TextBox">
+		<location x="478" y="614"/>
+	</widget>
+	<widget field="Right Current draw" type="Number" class="edu.wpi.first.smartdashboard.gui.elements.TextBox">
+		<location x="493" y="601"/>
+	</widget>
+	<widget field="High Gear" type="Boolean" class="edu.wpi.first.smartdashboard.gui.elements.BooleanBox">
+		<location x="79" y="11"/>
+		<width>93</width>
+		<height>78</height>
+	</widget>
+	<static-widget class="edu.wpi.first.smartdashboard.gui.elements.CameraServerViewer">
+		<location x="783" y="1"/>
+		<width>586</width>
+		<height>430</height>
+		<property name="Camera Choice" value="USB Camera 1"/>
+	</static-widget>
+	<static-widget class="edu.wpi.first.smartdashboard.gui.elements.CameraServerViewer">
+		<location x="148" y="2"/>
+		<width>630</width>
+		<height>459</height>
+		<property name="Camera Choice" value=""/>
+	</static-widget>
+	<widget field="LR" type="String Chooser" class="edu.wpi.first.smartdashboard.gui.elements.Chooser">
+		<location x="31" y="2"/>
+	</widget>
+	<widget field="LL" type="String Chooser" class="edu.wpi.first.smartdashboard.gui.elements.Chooser">
+		<location x="16" y="12"/>
+	</widget>
+	<widget field="RR" type="String Chooser" class="edu.wpi.first.smartdashboard.gui.elements.Chooser">
+		<location x="4" y="11"/>
+	</widget>
+	<widget field="Starting Position" type="String Chooser" class="edu.wpi.first.smartdashboard.gui.elements.Chooser">
+		<location x="11" y="16"/>
+	</widget>
+	<widget field="RL" type="String Chooser" class="edu.wpi.first.smartdashboard.gui.elements.Chooser">
+		<location x="13" y="30"/>
+	</widget>
+</dashboard>
+<live-window>
+</live-window>
+	<hidden field="Right PID Controller"/>
+	<hidden field="VPID Right kI"/>
+	<hidden field="VPID Left kI"/>
+	<hidden field="Turn PID Error"/>
+	<hidden field="Auto Delay"/>
+	<hidden field="PIDController[2]"/>
+	<hidden field="Move PID Error"/>
+	<hidden field="Turn PID"/>
+	<hidden field="Left PID Controller"/>
+	<hidden field="Left Solenoid Open"/>
+	<hidden field="Angle"/>
+	<hidden field="Turn PID Result"/>
+	<hidden field="Move PID"/>
+	<hidden field="VPID kF"/>
+	<hidden field="Move PID Output"/>
+	<hidden field="PIDController[1]"/>
+	<hidden field="VPID Right kP"/>
+	<hidden field="VPID Left kP"/>
+	<hidden field="Turn PID Output"/>
+	<hidden field="Move PID Result"/>
+</xml>

Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java

@@ -7,22 +7,23 @@
 
 package org.usfirst.frc.team199.Robot2018;
 
-import org.usfirst.frc.team199.Robot2018.commands.CloseIntake;
+import org.usfirst.frc.team199.Robot2018.commands.AutoLift;
 import org.usfirst.frc.team199.Robot2018.commands.FindTurnTimeConstant;
 import org.usfirst.frc.team199.Robot2018.commands.IntakeCube;
-import org.usfirst.frc.team199.Robot2018.commands.OpenIntake;
+import org.usfirst.frc.team199.Robot2018.commands.MoveLift;
+import org.usfirst.frc.team199.Robot2018.commands.MoveLiftWithPID;
 import org.usfirst.frc.team199.Robot2018.commands.OuttakeCube;
 import org.usfirst.frc.team199.Robot2018.commands.PIDMove;
 import org.usfirst.frc.team199.Robot2018.commands.PIDTurn;
 import org.usfirst.frc.team199.Robot2018.commands.ResetEncoders;
-import org.usfirst.frc.team199.Robot2018.commands.RunLift;
 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.ToggleLeftIntake;
-import org.usfirst.frc.team199.Robot2018.commands.ToggleRightIntake;
+import org.usfirst.frc.team199.Robot2018.commands.StopIntake;
+import org.usfirst.frc.team199.Robot2018.commands.ToggleIntake;
 import org.usfirst.frc.team199.Robot2018.commands.UpdatePIDConstants;
+import org.usfirst.frc.team199.Robot2018.subsystems.LiftInterface.LiftHeight;
 
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
@@ -48,6 +49,9 @@ public class OI {
 	private JoystickButton resetEncButton;
 	private JoystickButton moveLiftUpButton;
 	private JoystickButton moveLiftDownButton;
+	private JoystickButton moveLiftPIDUpButton;
+	private JoystickButton moveLiftPIDDownButton;
+	private JoystickButton testLiftPID;
 	private JoystickButton findTurnTimeConstantButton;
 	private JoystickButton updatePIDConstantsButton;
 	private JoystickButton updateEncoderDPPButton;
@@ -61,6 +65,8 @@ public class OI {
 	private JoystickButton outakeCubeButton;
 	private JoystickButton toggleLeftIntakeButton;
 	private JoystickButton toggleRightIntakeButton;
+	private JoystickButton toggleIntakeButton;
+	private JoystickButton stopIntakeButton;
 
 	public int getButton(String key, int def) {
 		if (!SmartDashboard.containsKey("Button/" + key)) {
@@ -92,6 +98,10 @@ public OI(Robot robot) {
 		findTurnTimeConstantButton
 				.whenPressed(new FindTurnTimeConstant(robot, Robot.dt, Robot.rmap.fancyGyro, Robot.sd));
 
+		testLiftPID = new JoystickButton(leftJoy, getButton("Test Lift PID", 5));
+		testLiftPID.whenPressed(
+				new AutoLift(Robot.lift, SmartDashboard.getString("Lift Targ Height", LiftHeight.SWITCH.toString())));
+
 		rightJoy = new Joystick(1);
 		shiftHighGearButton = new JoystickButton(rightJoy, getButton("Shift High Gear", 4));
 		shiftHighGearButton.whenPressed(new ShiftHighGear());
@@ -104,34 +114,46 @@ public OI(Robot robot) {
 		updateEncoderDPPButton.whenPressed(new SetDistancePerPulse());
 
 		moveLiftUpButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Up", 10));
-		moveLiftUpButton.whileHeld(new RunLift(Robot.lift, true));
+		moveLiftUpButton.whileHeld(new MoveLift(Robot.lift, true));
 		moveLiftDownButton = new JoystickButton(rightJoy, getButton("Run Lift Motor Down", 11));
-		moveLiftDownButton.whileHeld(new RunLift(Robot.lift, false));
+		moveLiftDownButton.whileHeld(new MoveLift(Robot.lift, false));
 
 		manipulator = new Joystick(2);
-		if (manipulator.getButtonCount() == 0) {
-			System.err.println(
-					"ERROR: manipulator does not appear to be plugged in. Disabling intake code. Restart code with manipulator plugged in to enable intake code");
-		} else {
-			closeIntakeButton = new JoystickButton(manipulator, getButton("Close Intake Button", 1));
-			closeIntakeButton.whenPressed(new CloseIntake());
-			openIntakeButton = new JoystickButton(manipulator, getButton("Open Intake Button", 2));
-			openIntakeButton.whenPressed(new OpenIntake());
-			// raiseIntake = new JoystickButton(manipulator, getButton("Raise Intake
-			// Button", 3));
-			// raiseIntake.whenPressed(new RaiseIntake());
-			// lowerIntake = new JoystickButton(manipulator, getButton("Lower Intake
-			// Button", 4));
-			// lowerIntake.whenPressed(new LowerIntake());
-			intakeCubeButton = new JoystickButton(manipulator, getButton("Intake Button", 5));
-			intakeCubeButton.whenPressed(new IntakeCube());
-			outakeCubeButton = new JoystickButton(manipulator, getButton("Outake Button", 6));
-			outakeCubeButton.whenPressed(new OuttakeCube());
-			toggleLeftIntakeButton = new JoystickButton(manipulator, getButton("Toggle Left Intake Button", 3));
-			toggleLeftIntakeButton.whenPressed(new ToggleLeftIntake());
-			toggleRightIntakeButton = new JoystickButton(manipulator, getButton("Toggle Right Intake Button", 4));
-			toggleRightIntakeButton.whenPressed(new ToggleRightIntake());
-		}
+		// if (manipulator.getButtonCount() == 0) {
+		// System.err.println(
+		// "ERROR: manipulator does not appear to be plugged in. Disabling intake code.
+		// Restart code with manipulator plugged in to enable intake code");
+		// } else {
+
+		// closeIntakeButton = new JoystickButton(manipulator, getButton("Close Intake
+		// Button", 1));
+		// closeIntakeButton.whenPressed(new CloseIntake());
+		// openIntakeButton = new JoystickButton(manipulator, getButton("Open Intake
+		// Button", 2));
+		// openIntakeButton.whenPressed(new OpenIntake());
+
+		moveLiftPIDUpButton = new JoystickButton(manipulator, getButton("Run Lift PID Up", 4));
+		moveLiftPIDUpButton.whileHeld(new MoveLiftWithPID(Robot.lift, true));
+		moveLiftPIDDownButton = new JoystickButton(manipulator, getButton("Run Lift PID Down", 1));
+		moveLiftPIDDownButton.whileHeld(new MoveLiftWithPID(Robot.lift, false));
+
+		toggleIntakeButton = new JoystickButton(manipulator, getButton("Toggle Intake Button", 2));
+		toggleIntakeButton.whenPressed(new ToggleIntake());
+
+		stopIntakeButton = new JoystickButton(manipulator, getButton("Stop Intake Button", 3));
+		stopIntakeButton.whenPressed(new StopIntake());
+
+		intakeCubeButton = new JoystickButton(manipulator, getButton("Intake Button", 5));
+		intakeCubeButton.whenPressed(new IntakeCube());
+		outakeCubeButton = new JoystickButton(manipulator, getButton("Outake Button", 6));
+		outakeCubeButton.whenPressed(new OuttakeCube());
+		// toggleLeftIntakeButton = new JoystickButton(manipulator, getButton("Toggle
+		// Left Intake Button", 3));
+		// toggleLeftIntakeButton.whenPressed(new ToggleLeftIntake());
+		// toggleRightIntakeButton = new JoystickButton(manipulator, getButton("Toggle
+		// Right Intake Button", 4));
+		// toggleRightIntakeButton.whenPressed(new ToggleRightIntake());
+		// }
 
 	}