diff --git a/AAAScripts/scripts.txt b/AAAScripts/scripts.txt index a613e59..de56173 100644 --- 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 index 07fccca..37615a7 100644 --- a/CompSD.xml +++ b/CompSD.xml @@ -1,12 +1,25 @@ + + + 552 + 403 + + + + + + + + + 118 59 - + 96 30 @@ -15,19 +28,6 @@ 113 44 - - - 571 - 448 - - - - - 586 - 438 - - - @@ -129,13 +129,13 @@ - + - + - + @@ -153,12 +153,12 @@ 46 293 - + 305 323 - + @@ -251,7 +251,7 @@ 20 326 - + 338 974 diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java index 5c150d8..39ed5f3 100644 --- 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 index bfb4bef..6af3cae 100644 --- 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 index 4dd28da..8e17b25 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java @@ -109,7 +109,7 @@ 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; } @@ -117,13 +117,13 @@ public static boolean isValidCommand(String instruction, String args, int lineNu 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 index 608c17f..03c51a9 100644 --- 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)`
`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.
Negative angle for counterclockwise. | `turn (36,48)`
`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 index fb3fdbc..4615dc3 100644 --- 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 index bc23e32..3a7fa9f 100644 --- 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 index 11bf36a..91f0ead 100644 --- 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 index 2a7081d..8056170 100644 --- 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 index e4946c4..88aa907 100644 --- 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 index 67eb5cf..94e6d1b 100644 --- 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 index b955133..7c526db 100644 --- 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 index 629f133..14593f4 100644 --- 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;