From 1cd440600f3af0637016f9cdd5832943d61ac89a Mon Sep 17 00:00:00 2001 From: Kevin Wamg <20215378+kevinzwang@users.noreply.github.com> Date: Wed, 7 Feb 2018 20:22:40 -0800 Subject: [PATCH 01/12] address issues #44 and 45 change problems in script parsing from warning to error and allow coordinate points with spaces between --- .../team199/Robot2018/autonomous/AutoUtils.java | 15 ++++++++++++++- .../team199/Robot2018/ParseScriptFileTest.java | 8 +++++--- 2 files changed, 19 insertions(+), 4 deletions(-) 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 ffb5e7a..61edaea 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java @@ -31,6 +31,19 @@ public static Map> parseScriptFile(String scriptFile // trim and remove extra whitespace just to make it neater line = line.trim().replaceAll("\\s+", " "); + // make coordinates with spaces also work + int parenIndex = line.indexOf("("); + while (parenIndex != -1) { + // removes all spaces between the parentheses + int endParenIndex = line.indexOf(")", parenIndex); + for (int i = line.indexOf(" ", parenIndex); i < endParenIndex && i != -1; i = line.indexOf(" ", i)) { + line = line.substring(0, i) + line.substring(i + 1); + } + + // finds next parentheses + parenIndex = line.indexOf("(", parenIndex + 1); + } + // if there's no instruction on this line, skip if (line.equals("")) { continue; @@ -168,7 +181,7 @@ else if (instruction.equals("jump")) { * @param message the message to log */ private static void logWarning (int lineNumber, String message) { - System.err.println("[WARNING] Line " + lineNumber + ": " + message); + System.err.println("[ERROR] Line " + lineNumber + ": " + message); } /** diff --git a/Robot2018/test/org/usfirst/frc/team199/Robot2018/ParseScriptFileTest.java b/Robot2018/test/org/usfirst/frc/team199/Robot2018/ParseScriptFileTest.java index ece0773..ca4b931 100644 --- a/Robot2018/test/org/usfirst/frc/team199/Robot2018/ParseScriptFileTest.java +++ b/Robot2018/test/org/usfirst/frc/team199/Robot2018/ParseScriptFileTest.java @@ -134,7 +134,7 @@ void test9() { @Test void test10() { String input = "CLRx:\n" - + "turn 0x4a"; // unfortunately, the program does not except hexadecimal xD + + "turn 0x4a"; // unfortunately, the program does not accept hexadecimal xD Map> output = AutoUtils.parseScriptFile(input); assertEquals(1, output.size()); @@ -148,7 +148,8 @@ void test11() { Map> output = AutoUtils.parseScriptFile(input); assertEquals(1, output.size()); - assertEquals(true, output.get("CLRx").isEmpty()); + assertEquals("moveto", output.get("CLRx").get(0)[0]); + assertEquals("(4,20) (18,47) 39", output.get("CLRx").get(0)[1]); } @Test @@ -168,7 +169,8 @@ void test13() { Map> output = AutoUtils.parseScriptFile(input); assertEquals(1, output.size()); - assertEquals(true, output.get("CLRx").isEmpty()); + assertEquals("moveto", output.get("CLRx").get(0)[0]); + assertEquals("(4,20) (18,47) 39", output.get("CLRx").get(0)[1]); } @Test From 6ea2559715a20f51c08196cab7242acac8bfdc28 Mon Sep 17 00:00:00 2001 From: Kevin Wamg <20215378+kevinzwang@users.noreply.github.com> Date: Sat, 10 Feb 2018 14:13:47 -0800 Subject: [PATCH 02/12] use replaceAll instead of for loop --- .../usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 61edaea..48d86d1 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java @@ -36,9 +36,8 @@ public static Map> parseScriptFile(String scriptFile while (parenIndex != -1) { // removes all spaces between the parentheses int endParenIndex = line.indexOf(")", parenIndex); - for (int i = line.indexOf(" ", parenIndex); i < endParenIndex && i != -1; i = line.indexOf(" ", i)) { - line = line.substring(0, i) + line.substring(i + 1); - } + String coord = line.substring(parenIndex + 1, endParenIndex); + line = line.substring(0, parenIndex + 1) + coord.replaceAll(" ", "") + line.substring(endParenIndex); // finds next parentheses parenIndex = line.indexOf("(", parenIndex + 1); From 7102df481f74464788ca3f12355725b5a51b3682 Mon Sep 17 00:00:00 2001 From: Kevin Wamg <20215378+kevinzwang@users.noreply.github.com> Date: Sun, 18 Feb 2018 14:20:11 -0800 Subject: [PATCH 03/12] modify changing and getting AutoUtils values PIDTurn and PIDMove can now move to an absolute value and they also update the position and angle based on encoder and gyro values instead of the assumed values --- .../team199/Robot2018/commands/AutoMove.java | 36 --------------- .../team199/Robot2018/commands/AutoTurn.java | 36 --------------- .../team199/Robot2018/commands/PIDMove.java | 46 +++++++++++++++++++ .../team199/Robot2018/commands/PIDTurn.java | 25 ++++++++++ 4 files changed, 71 insertions(+), 72 deletions(-) delete mode 100644 Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMove.java delete mode 100644 Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoTurn.java diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMove.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMove.java deleted file mode 100644 index 6ba1dd2..0000000 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMove.java +++ /dev/null @@ -1,36 +0,0 @@ -package org.usfirst.frc.team199.Robot2018.commands; - -import edu.wpi.first.wpilibj.command.Command; - -/** - * - */ -public class AutoMove extends Command { - - public AutoMove(double distance) { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); - } - - // Called just before this Command runs the first time - protected void initialize() { - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return false; - } - - // Called once after isFinished returns true - protected void end() { - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - } -} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoTurn.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoTurn.java deleted file mode 100644 index 1ce32a5..0000000 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoTurn.java +++ /dev/null @@ -1,36 +0,0 @@ -package org.usfirst.frc.team199.Robot2018.commands; - -import edu.wpi.first.wpilibj.command.Command; - -/** - * - */ -public class AutoTurn extends Command { - - public AutoTurn(double angle) { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); - } - - // Called just before this Command runs the first time - protected void initialize() { - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return false; - } - - // Called once after isFinished returns true - protected void end() { - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - } -} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java index 7619b5c..f5d553d 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java @@ -2,6 +2,7 @@ import org.usfirst.frc.team199.Robot2018.Robot; import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface; +import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils; import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface; import edu.wpi.first.wpilibj.PIDController; @@ -32,6 +33,8 @@ public class PIDMove extends Command implements PIDOutput { * @param dt * the Drivetrain (for actual code) or a DrivetrainInterface (for * testing) + * @param sd + * the SmartDashboard * @param avg * the PIDSourceAverage of the DT's two Encoders */ @@ -50,6 +53,42 @@ public PIDMove(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, kf, avg, this); } + /** + * Constructs this command with a new PIDController. Sets all of the + * controller's PID constants based on SD prefs. Sets the controller's PIDSource + * to the encoder average object and sets its PIDOutput to this command which + * implements PIDOutput's pidWrite() method. + * + * @param point + * the target point in inches, absolute distance from the starting + * point + * @param dt + * the Drivetrain (for actual code) or a DrivetrainInterface (for + * testing) + * @param sd + * the SmartDashboard + * @param avg + * the PIDSorceAverage of the DT's two Encoders + */ + public PIDMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource avg) { + double dx = point[0] - AutoUtils.position.getX(); + double dy = point[1] - AutoUtils.position.getY(); + + double dist = Math.sqrt(dx * dx + dy * dy); // pythagorean theorem to find distance + + this.target = dist; + this.dt = dt; + this.avg = avg; + if (Robot.dt != null) { + requires(Robot.dt); + } + moveController = new PIDController(sd.getConst("MovekP", 1), sd.getConst("MovekI", 0), sd.getConst("MovekD", 0), + avg, this); + double kf = 1 / (dt.getCurrentMaxSpeed() * sd.getConst("Default PID Update Time", 0.05)); + moveController = new PIDController(sd.getConst("MovekP", 1), sd.getConst("MovekI", 0), sd.getConst("MovekD", 0), + kf, avg, this); + } + /** * Called just before this Command runs the first time. Resets the distance * encoders, sets the moveController's settings, and then enables it. @@ -107,6 +146,13 @@ protected void end() { moveController.disable(); System.out.println("End"); // moveController.free(); + + double angle = Math.toRadians(AutoUtils.position.getRot()); + double dist = avg.pidGet(); + double x = Math.cos(angle) * dist; + double y = Math.sin(angle) * dist; + AutoUtils.position.changeX(x); + AutoUtils.position.changeY(y); } /** diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java index 9677eb0..8c4be23 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java @@ -2,6 +2,7 @@ import org.usfirst.frc.team199.Robot2018.Robot; import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface; +import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils; import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface; import edu.wpi.first.wpilibj.PIDController; @@ -60,6 +61,28 @@ public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, // tim = new Timer(); } + public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs) { + this.dt = dt; + this.ahrs = ahrs; + + double dx = point[0] - AutoUtils.position.getX(); + double dy = point[1] - AutoUtils.position.getY(); + + double absTurn = Math.toDegrees(Math.atan(dx / dy)); + target = absTurn - AutoUtils.position.getRot(); + + if (Robot.dt != null) { + requires(Robot.dt); + } + // calculates the maximum turning speed in degrees/sec based on the max linear + // speed in inches/s and the distance (inches) between sides of the DT + double maxTurnSpeed = dt.getCurrentMaxSpeed() * 360 / (Math.PI * sd.getConst("Distance Between Wheels", 26.25)); + double kf = 1 / (maxTurnSpeed * sd.getConst("Default PID Update Time", 0.05)); + turnController = new PIDController(sd.getConst("TurnkP", 1), sd.getConst("TurnkI", 0), sd.getConst("TurnkD", 0), + kf, ahrs, this); + // tim = new Timer(); + } + /** * Called just before this Command runs the first time. Resets the gyro, sets * the turnController's settings, and then enables it. @@ -137,6 +160,8 @@ protected void end() { SmartDashboard.putNumber("Turn PID Result", turnController.get()); SmartDashboard.putNumber("Turn PID Error", turnController.getError()); // turnController.free(); + + AutoUtils.position.changeRot(dt.getAHRSAngle()); } /** From 4a8dbdb9d2864ffee7dbf6dd107974ec452c76d9 Mon Sep 17 00:00:00 2001 From: Kevin Wamg <20215378+kevinzwang@users.noreply.github.com> Date: Sun, 18 Feb 2018 14:49:04 -0800 Subject: [PATCH 04/12] add documentation --- .../team199/Robot2018/commands/PIDMove.java | 2 +- .../team199/Robot2018/commands/PIDTurn.java | 26 ++++++++++++++++--- 2 files changed, 23 insertions(+), 5 deletions(-) diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java index 02c5141..1baa872 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java @@ -144,7 +144,7 @@ protected void end() { moveController.disable(); System.out.println("End"); // moveController.free(); - + double angle = Math.toRadians(AutoUtils.position.getRot()); double dist = avg.pidGet(); double x = Math.cos(angle) * dist; diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java index 8c4be23..64ba67c 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java @@ -61,16 +61,34 @@ public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, // tim = new Timer(); } + /** + * Constructs this command with a new PIDController. Sets all of the + * controller's PID constants based on SD prefs. Sets the controller's PIDSource + * to the AHRS (gyro) object and sets its PIDOutput to this command which + * implements PIDOutput's pidWrite() method. + * + * @param point + * the target point (in inches) to turn to, relative to the starting + * position + * @param dt + * the Drivetrain (for actual code) or a DrivetrainInterface (for + * testing) + * @param ahrs + * the AHRS (gyro) + * @param sd + * the Smart Dashboard reference, or a SmartDashboardInterface for + * testing + */ public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs) { this.dt = dt; this.ahrs = ahrs; - + double dx = point[0] - AutoUtils.position.getX(); double dy = point[1] - AutoUtils.position.getY(); - + double absTurn = Math.toDegrees(Math.atan(dx / dy)); target = absTurn - AutoUtils.position.getRot(); - + if (Robot.dt != null) { requires(Robot.dt); } @@ -160,7 +178,7 @@ protected void end() { SmartDashboard.putNumber("Turn PID Result", turnController.get()); SmartDashboard.putNumber("Turn PID Error", turnController.getError()); // turnController.free(); - + AutoUtils.position.changeRot(dt.getAHRSAngle()); } From 237bde6cec2298936cb09ec3d72a4f8fc82563f3 Mon Sep 17 00:00:00 2001 From: Kevin Wamg <20215378+kevinzwang@users.noreply.github.com> Date: Sun, 18 Feb 2018 15:41:44 -0800 Subject: [PATCH 05/12] use absolute values in AutoMoveTo --- .../Robot2018/commands/AutoMoveTo.java | 20 ++--------- .../team199/Robot2018/commands/PIDTurn.java | 33 +++++++++++++++++-- 2 files changed, 34 insertions(+), 19 deletions(-) diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java index ec04c61..76e0333 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java @@ -24,25 +24,11 @@ public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface for (String arg : args) { if (AutoUtils.isDouble(arg)) { rotation = Double.valueOf(arg); - double relrotation = rotation - AutoUtils.position.getRot(); - addSequential(new PIDTurn(relrotation, dt, sd, pidMoveSrc)); - AutoUtils.position.setRot(rotation); + addSequential(new PIDTurn(rotation, dt, sd, pidMoveSrc, true)); } else if (AutoUtils.isPoint(arg)) { point = AutoUtils.parsePoint(arg); - double deltaX = point[0] - AutoUtils.position.getX(); - double deltaY = point[1] - AutoUtils.position.getY(); - double atan = Math.toDegrees(Math.atan(deltaX / deltaY)); - double relrotation = atan - AutoUtils.position.getRot(); - addSequential(new PIDTurn(relrotation, dt, sd, pidMoveSrc)); - double dX2 = deltaX * deltaX; - double dY2 = deltaY * deltaY; - double distance = Math.sqrt(dX2 + dY2); - addSequential(new PIDMove(distance, dt, sd, pidMoveSrc)); - double x = AutoUtils.position.getX(); - double y = AutoUtils.position.getY(); - AutoUtils.position.setX(point[0]); - AutoUtils.position.setY(point[1]); - AutoUtils.position.setRot(Math.toDegrees(Math.atan((point[0] - x) / (point[1] - y)))); + addSequential(new PIDTurn(point, dt, sd, pidMoveSrc)); + addSequential(new PIDMove(point, dt, sd, pidMoveSrc)); } else { throw new IllegalArgumentException(); } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java index 64ba67c..b9282ef 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java @@ -42,13 +42,20 @@ public class PIDTurn extends Command implements PIDOutput { * @param sd * the Smart Dashboard reference, or a SmartDashboardInterface for * testing + * @param absolute + * whether the target passed is absolute or relative */ - public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs) { + public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs, boolean absolute) { // Use requires() here to declare subsystem dependencies - target = targ; this.dt = dt; this.ahrs = ahrs; + if (absolute) { + target = targ - AutoUtils.position.getRot(); + } else { + target = targ; + } + if (Robot.dt != null) { requires(Robot.dt); } @@ -61,6 +68,28 @@ public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, // tim = new Timer(); } + /** + * Constructs this command with a new PIDController. Sets all of the + * controller's PID constants based on SD prefs. Sets the controller's PIDSource + * to the AHRS (gyro) object and sets its PIDOutput to this command which + * implements PIDOutput's pidWrite() method. + * + * @param targ + * the target bearing (in degrees) to turn to (so negative if turning + * left, positive if turning right) + * @param dt + * the Drivetrain (for actual code) or a DrivetrainInterface (for + * testing) + * @param ahrs + * the AHRS (gyro) + * @param sd + * the Smart Dashboard reference, or a SmartDashboardInterface for + * testing + */ + public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs) { + this(targ, dt, sd, ahrs, false); + } + /** * Constructs this command with a new PIDController. Sets all of the * controller's PID constants based on SD prefs. Sets the controller's PIDSource From 748d4a0ff74ea88a4b988161902332271e97c40d Mon Sep 17 00:00:00 2001 From: Kevin Wamg <20215378+kevinzwang@users.noreply.github.com> Date: Sun, 18 Feb 2018 15:52:35 -0800 Subject: [PATCH 06/12] modify RunScript to use absolute values --- .../frc/team199/Robot2018/commands/RunScript.java | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) 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 2c0bc59..bef69bf 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunScript.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunScript.java @@ -25,16 +25,17 @@ public RunScript(String scriptName) { addSequential(new AutoMoveTo(cmdArgs.split(" "))); break; case "turn": - double rotation = Double.parseDouble(cmdArgs); - addSequential(new PIDTurn(rotation, Robot.dt, Robot.sd, Robot.dt.getGyro())); - AutoUtils.position.setRot(rotation); + if (AutoUtils.isPoint(cmdArgs)) { + double[] point = AutoUtils.parsePoint(cmdArgs); + addSequential(new PIDTurn(point, Robot.dt, Robot.sd, Robot.dt.getGyro())); + } else { + double rotation = Double.parseDouble(cmdArgs); + addSequential(new PIDTurn(rotation, Robot.dt, Robot.sd, Robot.dt.getGyro())); + } break; case "move": double distance = Double.parseDouble(cmdArgs); - addSequential(new PIDMove(distance, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg())); - AutoUtils.position.setX(distance * Math.sin(Math.toRadians(AutoUtils.position.getRot()))); - AutoUtils.position.setY(distance * Math.cos(Math.toRadians(AutoUtils.position.getRot()))); break; case "switch": addSequential(new EjectToSwitch()); From 5b6d7c6be9eb38df2f7fd16c5d72dfbf451dbfd7 Mon Sep 17 00:00:00 2001 From: Kevin Wamg <20215378+kevinzwang@users.noreply.github.com> Date: Sun, 18 Feb 2018 15:56:01 -0800 Subject: [PATCH 07/12] fix what alex fixed in his code --- .../src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java index ceedb1c..3d61e4e 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java @@ -81,8 +81,6 @@ public PIDMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface s if (Robot.dt != null) { requires(Robot.dt); } - moveController = new PIDController(sd.getConst("MovekP", 1), sd.getConst("MovekI", 0), sd.getConst("MovekD", 0), - avg, this); double kf = 1 / (dt.getCurrentMaxSpeed() * sd.getConst("Default PID Update Time", 0.05)); moveController = new PIDController(sd.getConst("MovekP", 1), sd.getConst("MovekI", 0), sd.getConst("MovekD", 0), kf, avg, this); From f8c3503b9864a644ef32d40a8f42e869f92d6d61 Mon Sep 17 00:00:00 2001 From: Kevin Wamg <20215378+kevinzwang@users.noreply.github.com> Date: Sun, 18 Feb 2018 16:21:34 -0800 Subject: [PATCH 08/12] modify atan to atan2 --- .../src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java index dae4587..cf5925c 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java @@ -115,7 +115,7 @@ public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface s double dx = point[0] - AutoUtils.position.getX(); double dy = point[1] - AutoUtils.position.getY(); - double absTurn = Math.toDegrees(Math.atan(dx / dy)); + double absTurn = Math.toDegrees(Math.atan2(dx, dy)); target = absTurn - AutoUtils.position.getRot(); if (Robot.dt != null) { From f6c2870899893c0dc7d7a2a6f11424423d8bc23e Mon Sep 17 00:00:00 2001 From: Kevin Wamg <20215378+kevinzwang@users.noreply.github.com> Date: Sun, 18 Feb 2018 16:23:25 -0800 Subject: [PATCH 09/12] modify atan2 to pass in the right values --- .../src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java index cf5925c..0f063b1 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java @@ -115,7 +115,7 @@ public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface s double dx = point[0] - AutoUtils.position.getX(); double dy = point[1] - AutoUtils.position.getY(); - double absTurn = Math.toDegrees(Math.atan2(dx, dy)); + double absTurn = Math.toDegrees(Math.atan2(dy, dx)); target = absTurn - AutoUtils.position.getRot(); if (Robot.dt != null) { From 83cb30f7bb5db762301c51231b425a9d68b25344 Mon Sep 17 00:00:00 2001 From: lhmcgann Date: Sun, 18 Feb 2018 17:45:51 -0800 Subject: [PATCH 10/12] changes to make intake work --- Robot2018/lib/User_Libraries.properties | 4 ++-- .../usfirst/frc/team199/Robot2018/RobotMap.java | 10 ++++------ .../frc/team199/Robot2018/commands/IntakeCube.java | 2 +- .../frc/team199/Robot2018/commands/OutakeCube.java | 8 ++++++-- .../team199/Robot2018/subsystems/IntakeEject.java | 14 +++++++------- 5 files changed, 20 insertions(+), 18 deletions(-) diff --git a/Robot2018/lib/User_Libraries.properties b/Robot2018/lib/User_Libraries.properties index dacc870..284ff01 100644 --- a/Robot2018/lib/User_Libraries.properties +++ b/Robot2018/lib/User_Libraries.properties @@ -1,2 +1,2 @@ -#Sun Feb 18 11:01:27 PST 2018 -C\:\\Users\\dean\\git\\RobotCode2018\\Robot2018\\lib\\libpathfinderjava.so=4087ee38c338446b8305f67716260df4 +#Sun Feb 18 17:11:52 PST 2018 +C\:\\Users\\Student\\git\\RobotCode2018\\Robot2018\\lib\\libpathfinderjava.so=4087ee38c338446b8305f67716260df4 diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java index a627bdc..339e086 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java @@ -130,12 +130,10 @@ public RobotMap() { leftIntakeMotor = new VictorSP(getPort("IntakeLeftVictorSP", 9)); rightIntakeMotor = new VictorSP(getPort("IntakeRightVictorSP", 8)); - // leftIntakeHorizontalSolenoid = new - // DoubleSolenoid(getPort("IntakeLeftHorizontalSolenoidPort1", 2), - // getPort("IntakeLeftHorizontalSolenoidPort2", 3)); - // rightIntakeHorizontalSolenoid = new - // DoubleSolenoid(getPort("IntakeRightHorizontalSolenoidPort1", 4), - // getPort("IntakeRightHorizontalSolenoidPort2", 5)); + leftIntakeHorizontalSolenoid = new DoubleSolenoid(getPort("IntakeLeftHorizontalSolenoidPort1", 2), + getPort("IntakeLeftHorizontalSolenoidPort2", 3)); + rightIntakeHorizontalSolenoid = new DoubleSolenoid(getPort("IntakeRightHorizontalSolenoidPort1", 4), + getPort("IntakeRightHorizontalSolenoidPort2", 5)); // leftIntakeVerticalSolenoid = new // DoubleSolenoid(getPort("IntakeLeftVerticalSolenoidPort1", 6), // getPort("IntakeLeftVerticalSolenoidPort2", 7)); diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java index 5557563..fa127ba 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/IntakeCube.java @@ -20,7 +20,7 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run protected void execute() { - Robot.intakeEject.runIntake(1); + Robot.intakeEject.runIntake(-1); } // Make this return true when this Command no longer needs to run execute() diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OutakeCube.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OutakeCube.java index e68dea4..ee95d98 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OutakeCube.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/OutakeCube.java @@ -20,24 +20,28 @@ public OutakeCube() { // Called just before this Command runs the first time protected void initialize() { tim = new Timer(); + tim.reset(); + tim.start(); } // Called repeatedly when this Command is scheduled to run protected void execute() { - Robot.intakeEject.runIntake(-1); + Robot.intakeEject.runIntake(1); } // Make this return true when this Command no longer needs to run execute() protected boolean isFinished() { - return tim.get() > Robot.getConst("Outake Time", 1); + return tim.get() > Robot.getConst("Outake Time", 0.5); } // Called once after isFinished returns true protected void end() { + Robot.intakeEject.runIntake(0); } // Called when another command which requires one or more of the same // subsystems is scheduled to run protected void interrupted() { + end(); } } 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 77cdfbf..5a7b794 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java @@ -48,9 +48,9 @@ public double getRightIntakeSpeed() { * */ public boolean hasCube() { - return pdp.getCurrent(Robot.rmap.getPort("PDP Intake Left Channel", 4)) > Robot.getConst("Max Current", 38) - && pdp.getCurrent(Robot.rmap.getPort("PDP Intake Right Channel", 11)) > Robot.getConst("Max Current", - 38); + return pdp.getCurrent(Robot.rmap.getPort("PDP Intake Left Channel", 4)) > Robot.getConst("Max Current", 39) + || pdp.getCurrent(Robot.rmap.getPort("PDP Intake Right Channel", 11)) > Robot.getConst("Max Current", + 39); } /** @@ -124,9 +124,9 @@ public void lowerIntake() { } /** - * Opens the intake + * Closes the intake */ - public void openIntake() { + public void closeIntake() { DoubleSolenoid.Value leftSet = Robot.getBool("Intake Left Horizontal Solenoid Inverted", false) ? DoubleSolenoid.Value.kReverse : DoubleSolenoid.Value.kForward; @@ -138,9 +138,9 @@ public void openIntake() { } /** - * Closes the intake + * Opens the intake */ - public void closeIntake() { + public void openIntake() { DoubleSolenoid.Value leftSet = Robot.getBool("Intake Left Horizontal Solenoid Inverted", false) ? DoubleSolenoid.Value.kForward : DoubleSolenoid.Value.kReverse; From b507e2afb69627258676cc2143f9d31e13f17b3a Mon Sep 17 00:00:00 2001 From: DriverStationComputer Date: Mon, 19 Feb 2018 10:20:34 -0800 Subject: [PATCH 11/12] start testing auto --- AAAScripts/scripts.txt | 22 ++++++++-------- Robot2018/lib/User_Libraries.properties | 2 +- .../usfirst/frc/team199/Robot2018/Robot.java | 4 +-- .../Robot2018/autonomous/AutoUtils.java | 25 ++++++++++--------- .../team199/Robot2018/commands/PIDMove.java | 2 +- .../team199/Robot2018/commands/PIDTurn.java | 2 +- 6 files changed, 29 insertions(+), 28 deletions(-) diff --git a/AAAScripts/scripts.txt b/AAAScripts/scripts.txt index b12474c..dcd8ade 100644 --- a/AAAScripts/scripts.txt +++ b/AAAScripts/scripts.txt @@ -36,7 +36,7 @@ RxLx: #------------------------------------------------- #Exchange -RxxE +RxxE: moveto (0, 188.4) (-210.6, 188.4) (-210.6, 44.4) (-123.6, 44.4) (-123.6, -39.6) 180 #Curve around switch for baseline #then go to exchange exchange @@ -144,7 +144,7 @@ LxRx: #------------------------------------------------- #Exchange -LxxE +LxxE: moveto (0, 188.4) (210.6, 188.4) (210.6, 3.7) (124.8, 3.7) (124.8, -3.3) 180 #Curve around switch for baseline then go to exchange exchange @@ -244,13 +244,13 @@ moveto (48,80.375) #cross baseline CRxx: moveto (0,60) 90 #5 feet forward, turn 90 moveto (48,60) -90 #4 feet right,turn -90 -moveto(48,104.375) #7 feet forward until against switch enclosure, compensating for robot length +moveto (48,104.375) #7 feet forward until against switch enclosure, compensating for robot length switch #deploy switch CLxx: moveto (0,60) -90 #5 feet forward, turn -90 moveto (-48,60) 90 #4 feet left,turn 90 -moveto(-48,104.375) #7 feet forward until against switch enclosure,compensating for robot length +moveto (-48,104.375) #7 feet forward until against switch enclosure,compensating for robot length switch #deploy switch @@ -275,7 +275,7 @@ scale #deploy scale CRLx: moveto (0,60) 90 #5 feet forward, turn 90 moveto (48,60) -90 #4 feet right,turn -90 -moveto(48,104.375) #7 feet forward until against switch enclosure +moveto (48,104.375) #7 feet forward until against switch enclosure switch #deploy switch move -39 #move 39 inches back turn -90 #turn -90 degrees @@ -294,7 +294,7 @@ scale #deploy scale2 CRRx: moveto (0,60) 90 #5 feet forward, turn 90 moveto (48,60) -90 #4 feet right,turn -90 -moveto(48,104.375) #7 feet forward until against switch enclosure +moveto (48,104.375) #7 feet forward until against switch enclosure switch #deploy switch move -39 #move 39 inches back turn -90 #turn -90 degrees @@ -313,7 +313,7 @@ scale #deploy scale CLRx: moveto (0,60) -90 #5 feet forward, turn -90 moveto (-48,60) 90 #4 feet left,turn 90 -moveto(-48,104.375) #7 feet forward until against switch enclosure +moveto (-48,104.375) #7 feet forward until against switch enclosure switch #deploy switch move -39 #move 39 inches back turn -90 #turn -90 degrees @@ -331,7 +331,7 @@ scale #deploy scale CLLx: moveto (0,60) -90 #5 feet forward, turn -90 moveto (-48,60) 90 #4 feet left,turn 90 -moveto(-48,104.375) #7 feet forward until against switch enclosure +moveto (-48,104.375) #7 feet forward until against switch enclosure switch #deploy switch move -39 #move 39 inches back turn 90 #turn 90 degrees @@ -352,7 +352,7 @@ scale #deploy scale2 CRxE: moveto (0,60) 90 #5 feet forward, turn 90 moveto (48,60) -90 #4 feet right,turn -90 -moveto(48,104.375) #7 feet forward until against switch enclosure, compensating for robot length +moveto (48,104.375) #7 feet forward until against switch enclosure, compensating for robot length switch #deploy switch move -24 # 2 feet back turn -90 @@ -368,7 +368,7 @@ exchange CLxE: moveto (0,60) -90 #5 feet forward, turn -90 moveto (-48,60) 90 #3 feet left,turn 90 -moveto(-48,104.375) #7 feet forward until against switch enclosure,compensating for robot length +moveto (-48,104.375) #7 feet forward until against switch enclosure,compensating for robot length switch #deploy switch move -24 turn 90 @@ -383,7 +383,7 @@ exchange CxxE: moveto (0,60) -90 #5 feet forward, turn -90 moveto (-48,60) 90 #3 feet left,turn 90 -moveto(-48,104.375) #7 feet forward until against switch enclosure,compensating for robot length +moveto (-48,104.375) #7 feet forward until against switch enclosure,compensating for robot length move -96 #move 8 feet back turn 180 #turn 180 degrees to face the exchange exchange \ No newline at end of file diff --git a/Robot2018/lib/User_Libraries.properties b/Robot2018/lib/User_Libraries.properties index 0e4f44a..1ee5cb2 100644 --- a/Robot2018/lib/User_Libraries.properties +++ b/Robot2018/lib/User_Libraries.properties @@ -1,2 +1,2 @@ -#Sun Feb 18 15:40:25 PST 2018 +#Sun Feb 18 17:40:43 PST 2018 C\:\\Users\\dean\\git\\RobotCode2018\\Robot2018\\lib\\libpathfinderjava.so=4087ee38c338446b8305f67716260df4 diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java index 5c21509..b24495e 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java @@ -169,8 +169,8 @@ public void autonomousInit() { strategies.put(key, chooser.getSelected()); } - Autonomous auto = new Autonomous(startPos, strategies, autoDelay, fmsInput, false); - auto.start(); + Scheduler.getInstance().add(new Autonomous(startPos, strategies, autoDelay, fmsInput, false)); + // auto.start(); } /** 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 7f523a3..3d16272 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java @@ -24,15 +24,16 @@ public static Map> parseScriptFile(String scriptFile ArrayList currScript = new ArrayList(); String currScriptName = ""; - int count = 1; + int count = 0; for (String line : lines) { + count++; // remove comments int commentIndex = line.indexOf("#"); if (commentIndex != -1) line = line.substring(0, commentIndex); // trim and remove extra whitespace just to make it neater - line = line.trim().replaceAll("\\s+", " "); + line = line.trim().replaceAll("\\s+", " "); // make coordinates with spaces also work int parenIndex = line.indexOf("("); while (parenIndex != -1) { @@ -40,7 +41,7 @@ public static Map> parseScriptFile(String scriptFile int endParenIndex = line.indexOf(")", parenIndex); String coord = line.substring(parenIndex + 1, endParenIndex); line = line.substring(0, parenIndex + 1) + coord.replaceAll(" ", "") + line.substring(endParenIndex); - + // finds next parentheses parenIndex = line.indexOf("(", parenIndex + 1); } @@ -76,7 +77,7 @@ public static Map> parseScriptFile(String scriptFile currScript.add(command); } } - count++; + } // puts the last script in @@ -187,7 +188,7 @@ else if (instruction.equals("jump")) { * the message to log */ - private static void logWarning (int lineNumber, String message) { + private static void logWarning(int lineNumber, String message) { System.err.println("[ERROR] Line " + lineNumber + ": " + message); } @@ -246,13 +247,13 @@ public static double[] parsePoint(String cmdArgs) { if (AutoUtils.isPoint(cmdArgs)) { parentheseless = cmdArgs.substring(1, cmdArgs.length() - 1); pointparts = parentheseless.split(","); - try { - point[0] = Double.parseDouble(pointparts[0]); - point[1] = Double.parseDouble(pointparts[1]); - } catch (Exception e) { - point[0] = 1; - point[1] = 1; - } + try { + point[0] = Double.parseDouble(pointparts[0]); + point[1] = Double.parseDouble(pointparts[1]); + } catch (Exception e) { + point[0] = 1; + point[1] = 1; + } } return point; } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java index 3d61e4e..c1a0b76 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java @@ -100,7 +100,7 @@ public void initialize() { moveController.setOutputRange(-1.0, 1.0); moveController.setContinuous(false); moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 0.1)); - moveController.setSetpoint(Robot.getConst("Move Targ", 24)); + moveController.setSetpoint(target); moveController.enable(); // dt.enableVelocityPIDs(); diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java index 0f063b1..04bc3b0 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java @@ -149,7 +149,7 @@ protected void initialize() { turnController.setOutputRange(Robot.getConst("Output", 0.5) * -1, Robot.getConst("Output", 0.5)); turnController.setContinuous(true); turnController.setAbsoluteTolerance(Robot.getConst("TurnTolerance", 1)); - double newSetPoint = Robot.getConst("Turn Targ", 90) + dt.getAHRSAngle(); + double newSetPoint = target + dt.getAHRSAngle(); while (Math.abs(newSetPoint) > 180) { newSetPoint = newSetPoint - Math.signum(newSetPoint) * 360; } From a22dfc2456c0818a075c9577487b3c54495f79a0 Mon Sep 17 00:00:00 2001 From: doawelul Date: Mon, 19 Feb 2018 10:54:45 -0800 Subject: [PATCH 12/12] added indvidual horizontal toggling to left and right intake --- .../org/usfirst/frc/team199/Robot2018/OI.java | 35 +++++++++++------ .../Robot2018/commands/ToggleLeftIntake.java | 24 ++++++++++++ .../Robot2018/commands/ToggleRightIntake.java | 24 ++++++++++++ .../Robot2018/subsystems/IntakeEject.java | 39 +++++++++++++++++++ .../subsystems/IntakeEjectInterface.java | 10 +++++ 5 files changed, 120 insertions(+), 12 deletions(-) create mode 100644 Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ToggleLeftIntake.java create mode 100644 Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ToggleRightIntake.java diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java index 7e4e616..ced2550 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java @@ -7,6 +7,11 @@ package org.usfirst.frc.team199.Robot2018; +import org.usfirst.frc.team199.Robot2018.commands.CloseIntake; +import org.usfirst.frc.team199.Robot2018.commands.IntakeCube; +import org.usfirst.frc.team199.Robot2018.commands.LowerIntake; +import org.usfirst.frc.team199.Robot2018.commands.OpenIntake; +import org.usfirst.frc.team199.Robot2018.commands.OutakeCube; import org.usfirst.frc.team199.Robot2018.commands.PIDMove; import org.usfirst.frc.team199.Robot2018.commands.PIDTurn; import org.usfirst.frc.team199.Robot2018.commands.ResetEncoders; @@ -15,6 +20,8 @@ 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.UpdatePIDConstants; import edu.wpi.first.wpilibj.Joystick; @@ -50,6 +57,8 @@ public class OI { private JoystickButton lowerIntake; private JoystickButton intake; private JoystickButton outake; + private JoystickButton toggleLeftIntake; + private JoystickButton toggleRightIntake; public int getButton(String key, int def) { if (!SmartDashboard.containsKey("Button/" + key)) { @@ -90,22 +99,24 @@ public OI() { MoveLiftUpButton.whileHeld(new RunLift(Robot.lift, true)); MoveLiftDownButton.whileHeld(new RunLift(Robot.lift, false)); - // manipulator = new Joystick(2); - // closeIntake = new JoystickButton(manipulator, getButton("Close Intake - // Button", 1)); - // closeIntake.whenPressed(new CloseIntake()); - // openIntake = new JoystickButton(manipulator, getButton("Open Intake Button", - // 2)); - // openIntake.whenPressed(new OpenIntake()); + manipulator = new Joystick(2); + closeIntake = new JoystickButton(manipulator, getButton("Close Intake Button", 1)); + closeIntake.whenPressed(new CloseIntake()); + openIntake = new JoystickButton(manipulator, getButton("Open Intake Button", 2)); + openIntake.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()); - // intake = new JoystickButton(manipulator, getButton("Intake Button", 5)); - // intake.whenPressed(new IntakeCube()); - // outake = new JoystickButton(manipulator, getButton("Outake Button", 6)); - // outake.whenPressed(new OutakeCube()); + lowerIntake.whenPressed(new LowerIntake()); + intake = new JoystickButton(manipulator, getButton("Intake Button", 5)); + intake.whenPressed(new IntakeCube()); + outake = new JoystickButton(manipulator, getButton("Outake Button", 6)); + outake.whenPressed(new OutakeCube()); + toggleLeftIntake = new JoystickButton(manipulator, getButton("Toggle Left Intake Button", 3)); + toggleLeftIntake.whenPressed(new ToggleLeftIntake()); + toggleRightIntake = new JoystickButton(manipulator, getButton("Toggle Right Intake Button", 4)); + toggleRightIntake.whenPressed(new ToggleRightIntake()); } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ToggleLeftIntake.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ToggleLeftIntake.java new file mode 100644 index 0000000..6a4daec --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ToggleLeftIntake.java @@ -0,0 +1,24 @@ +package org.usfirst.frc.team199.Robot2018.commands; + +import org.usfirst.frc.team199.Robot2018.Robot; + +import edu.wpi.first.wpilibj.command.InstantCommand; + +/** + * + */ +public class ToggleLeftIntake extends InstantCommand { + + public ToggleLeftIntake() { + super(); + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.intakeEject); + } + + // Called once when the command executes + protected void initialize() { + Robot.intakeEject.toggleLeftIntake(); + } + +} diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ToggleRightIntake.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ToggleRightIntake.java new file mode 100644 index 0000000..e03d274 --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ToggleRightIntake.java @@ -0,0 +1,24 @@ +package org.usfirst.frc.team199.Robot2018.commands; + +import org.usfirst.frc.team199.Robot2018.Robot; + +import edu.wpi.first.wpilibj.command.InstantCommand; + +/** + * + */ +public class ToggleRightIntake extends InstantCommand { + + public ToggleRightIntake() { + super(); + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.intakeEject); + } + + // Called once when the command executes + protected void initialize() { + Robot.intakeEject.toggleRightIntake(); + } + +} 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 77cdfbf..c1e1ee6 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.PowerDistributionPanel; import edu.wpi.first.wpilibj.VictorSP; import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * @@ -123,6 +124,40 @@ public void lowerIntake() { rightVerticalSolenoid.set(rightSet); } + /** + * Toggles the left intake between open and closed + */ + public void toggleLeftIntake() { + DoubleSolenoid.Value set; + if (Robot.getBool("Bool/Left Horizontal Solenoid Open", true)) { + set = Robot.getBool("Intake Left Horizontal Solenoid Inverted", false) ? DoubleSolenoid.Value.kForward + : DoubleSolenoid.Value.kReverse; + } else { + set = Robot.getBool("Intake Left Horizontal Solenoid Inverted", false) ? DoubleSolenoid.Value.kReverse + : DoubleSolenoid.Value.kForward; + } + leftHorizontalSolenoid.set(set); + SmartDashboard.putBoolean("Bool/Left Horizontal Solenoid Open", + Robot.getBool("Left Horizontal Solenoid Open", true)); + } + + /** + * Toggles the right intake between open and closed + */ + public void toggleRightIntake() { + DoubleSolenoid.Value set; + if (Robot.getBool("Bool/Right Horizontal Solenoid Open", true)) { + set = Robot.getBool("Intake Right Horizontal Solenoid Inverted", false) ? DoubleSolenoid.Value.kForward + : DoubleSolenoid.Value.kReverse; + } else { + set = Robot.getBool("Intake Right Horizontal Solenoid Inverted", false) ? DoubleSolenoid.Value.kReverse + : DoubleSolenoid.Value.kForward; + } + rightHorizontalSolenoid.set(set); + SmartDashboard.putBoolean("Bool/Right Horizontal Solenoid Open", + Robot.getBool("right Horizontal Solenoid Open", true)); + } + /** * Opens the intake */ @@ -133,6 +168,8 @@ public void openIntake() { DoubleSolenoid.Value rightSet = Robot.getBool("Intake Right Horizontal Solenoid Inverted", false) ? DoubleSolenoid.Value.kReverse : DoubleSolenoid.Value.kForward; + SmartDashboard.putBoolean("Bool/Left Horizontal Solenoid Open", true); + SmartDashboard.putBoolean("Bool/Right Horizontal Solenoid Open", true); leftHorizontalSolenoid.set(leftSet); rightHorizontalSolenoid.set(rightSet); } @@ -147,6 +184,8 @@ public void closeIntake() { DoubleSolenoid.Value rightSet = Robot.getBool("Intake Right Horizontal Solenoid Inverted", false) ? DoubleSolenoid.Value.kForward : DoubleSolenoid.Value.kReverse; + SmartDashboard.putBoolean("Bool/Left Horizontal Solenoid Open", false); + SmartDashboard.putBoolean("Bool/Right Horizontal Solenoid Open", false); leftHorizontalSolenoid.set(leftSet); rightHorizontalSolenoid.set(rightSet); } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEjectInterface.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEjectInterface.java index f37bdde..01f5eb1 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEjectInterface.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEjectInterface.java @@ -64,6 +64,16 @@ public interface IntakeEjectInterface { */ public void lowerIntake(); + /** + * Toggles the left intake between open and closed + */ + public void toggleLeftIntake(); + + /** + * Toggles the right intake between open and closed + */ + public void toggleRightIntake(); + /** * Closes the intake */