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/14] 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/14] 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/14] 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/14] 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/14] 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/14] 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/14] 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/14] 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/14] 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 eed3d76ff62483c9bdd4a5872d69a68ebcbf7882 Mon Sep 17 00:00:00 2001 From: Kevin Wang Date: Mon, 19 Feb 2018 10:37:08 -0800 Subject: [PATCH 10/14] initial ds computer changes (#1) * added more functionality to intakeEject * add requires to default intake command * add end and interrupted to defaultIntake * changes to make intake work * start testing auto --- AAAScripts/scripts.txt | 22 +++++----- Robot2018/lib/User_Libraries.properties | 2 +- .../usfirst/frc/team199/Robot2018/Robot.java | 4 +- .../frc/team199/Robot2018/RobotMap.java | 10 ++--- .../Robot2018/autonomous/AutoUtils.java | 25 ++++++----- .../Robot2018/commands/DefaultIntake.java | 44 +++++++++++++++++++ .../Robot2018/commands/IntakeCube.java | 2 +- .../Robot2018/commands/OutakeCube.java | 8 +++- .../team199/Robot2018/commands/PIDMove.java | 2 +- .../team199/Robot2018/commands/PIDTurn.java | 2 +- .../Robot2018/subsystems/IntakeEject.java | 43 +++++++++++++----- .../subsystems/IntakeEjectInterface.java | 16 +++++++ 12 files changed, 133 insertions(+), 47 deletions(-) create mode 100644 Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/DefaultIntake.java 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/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/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/DefaultIntake.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/DefaultIntake.java new file mode 100644 index 0000000..b6fa578 --- /dev/null +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/DefaultIntake.java @@ -0,0 +1,44 @@ +package org.usfirst.frc.team199.Robot2018.commands; + +import org.usfirst.frc.team199.Robot2018.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class DefaultIntake extends Command { + + public DefaultIntake() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.intakeEject); + } + + // Called just before this Command runs the first time + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + // 1 and 5 represent the axes' index in driver station + Robot.intakeEject.runLeftIntake(Robot.oi.manipulator.getRawAxis(1)); + Robot.intakeEject.runRightIntake(Robot.oi.manipulator.getRawAxis(5)); + } + + // 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() { + 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/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/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; } 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 a8b44ff..5a7b794 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEject.java @@ -2,6 +2,7 @@ import org.usfirst.frc.team199.Robot2018.Robot; import org.usfirst.frc.team199.Robot2018.RobotMap; +import org.usfirst.frc.team199.Robot2018.commands.DefaultIntake; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.PowerDistributionPanel; @@ -24,6 +25,7 @@ public class IntakeEject extends Subsystem implements IntakeEjectInterface { * Set the default command for a subsystem here. */ public void initDefaultCommand() { + setDefaultCommand(new DefaultIntake()); } /** @@ -46,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); } /** @@ -60,6 +62,28 @@ public void stopIntake() { rightIntakeMotor.stopMotor(); } + /** + * Sets the left roller to run at the specified speed + * + * @param speed + * Speed the left motor should run at + */ + public void runLeftIntake(double speed) { + double actualSpeed = speed * Robot.getConst("Intake Motor Left Speed Multiplier", 1); + leftIntakeMotor.set(actualSpeed); + } + + /** + * Sets the left roller to run at the specified speed + * + * @param speed + * Speed the left motor should run at + */ + public void runRightIntake(double speed) { + double actualSpeed = speed * Robot.getConst("Intake Motor Right Speed Multiplier", 1); + rightIntakeMotor.set(actualSpeed); + } + /** * Spins the rollers * @@ -67,9 +91,8 @@ public void stopIntake() { * - positive -> rollers in, negative -> rollers out */ public void runIntake(double speed) { - double actualSpeed = speed * Robot.getConst("Intake Motor Speed Multiplier", 1); - leftIntakeMotor.set(actualSpeed); - rightIntakeMotor.set(actualSpeed); + runLeftIntake(speed); + runRightIntake(speed); } /** @@ -101,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; @@ -115,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; 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 0262727..f37bdde 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEjectInterface.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/IntakeEjectInterface.java @@ -30,6 +30,22 @@ public interface IntakeEjectInterface { */ public void stopIntake(); + /** + * Sets the left roller to run at the specified speed + * + * @param speed + * Speed the left motor should run at + */ + public void runLeftIntake(double speed); + + /** + * Sets the right roller to run at the specified speed + * + * @param speed + * Speed the right motor should run at + */ + public void runRightIntake(double speed); + /** * Spins the rollers * From edf90256ec7363a7d6c0de6eb97fd309b725b4ad Mon Sep 17 00:00:00 2001 From: DriverStationComputer Date: Mon, 19 Feb 2018 11:29:02 -0800 Subject: [PATCH 11/14] use sd interface instead of sd in PIDMove and PIDTurn --- .../src/org/usfirst/frc/team199/Robot2018/Robot.java | 9 +++++++++ .../team199/Robot2018/SmartDashboardInterface.java | 6 ++++++ .../frc/team199/Robot2018/commands/PIDMove.java | 12 +++++++----- .../frc/team199/Robot2018/commands/PIDTurn.java | 8 +++++--- 4 files changed, 27 insertions(+), 8 deletions(-) diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java index 5c21509..685d35d 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java @@ -25,6 +25,7 @@ import edu.wpi.first.wpilibj.CameraServer; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.PIDController; import edu.wpi.first.wpilibj.Preferences; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.command.Scheduler; @@ -69,6 +70,14 @@ public double getConst(String key, double def) { } return pref.getDouble("Const/" + key, def); } + + public void putData(String string, PIDController controller) { + SmartDashboard.putData(string, controller); + } + + public void putNumber(String string, double d) { + SmartDashboard.putNumber(string, d); + } /* * if (!SmartDashboard.containsKey("Const/" + key)) { if * (!SmartDashboard.putNumber("Const/" + key, def)) { diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/SmartDashboardInterface.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/SmartDashboardInterface.java index 18a9dc2..d7067ee 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/SmartDashboardInterface.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/SmartDashboardInterface.java @@ -1,5 +1,11 @@ package org.usfirst.frc.team199.Robot2018; +import edu.wpi.first.wpilibj.PIDController; + public interface SmartDashboardInterface { public double getConst(String key, double def); + + public void putData(String string, PIDController controller); + + public void putNumber(String string, double d); } 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..f1adcff 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java @@ -9,7 +9,6 @@ import edu.wpi.first.wpilibj.PIDOutput; import edu.wpi.first.wpilibj.PIDSource; import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * Drives the robot a certain target distance using PID. Implements PIDOutput in @@ -21,6 +20,7 @@ public class PIDMove extends Command implements PIDOutput { private DrivetrainInterface dt; private PIDController moveController; private PIDSource avg; + private SmartDashboardInterface sd; /** * Constructs this command with a new PIDController. Sets all of the @@ -43,13 +43,14 @@ public PIDMove(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, target = targ; this.dt = dt; this.avg = avg; + this.sd = sd; if (Robot.dt != null) { requires(Robot.dt); } double kf = 1 / (dt.getCurrentMaxSpeed() * sd.getConst("Default PID Update Time", 0.05)); moveController = new PIDController(sd.getConst("MovekP", 0.1), sd.getConst("MovekI", 0), sd.getConst("MovekD", 0), kf, avg, this); - SmartDashboard.putData("Move PID", moveController); + sd.putData("Move PID", moveController); } /** @@ -78,6 +79,7 @@ public PIDMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface s this.target = dist; this.dt = dt; this.avg = avg; + this.sd = sd; if (Robot.dt != null) { requires(Robot.dt); } @@ -114,8 +116,8 @@ public void initialize() { @Override protected void execute() { System.out.println("Enc Avg Dist: " + avg.pidGet()); - SmartDashboard.putNumber("Move PID Result", moveController.get()); - SmartDashboard.putNumber("Move PID Error", moveController.getError()); + sd.putNumber("Move PID Result", moveController.get()); + sd.putNumber("Move PID Error", moveController.getError()); } /** @@ -173,6 +175,6 @@ protected void interrupted() { @Override public void pidWrite(double output) { dt.arcadeDrive(output, 0); - SmartDashboard.putNumber("Move PID Output", output); + sd.putNumber("Move PID Output", output); } } 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..67d5b51 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java @@ -24,6 +24,7 @@ public class PIDTurn extends Command implements PIDOutput { private PIDSource ahrs; private Timer tim; private double lastTime; + private SmartDashboardInterface sd; /** * Constructs this command with a new PIDController. Sets all of the @@ -49,6 +50,7 @@ public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, // Use requires() here to declare subsystem dependencies this.dt = dt; this.ahrs = ahrs; + this.sd = sd; if (absolute) { target = targ - AutoUtils.position.getRot(); @@ -128,7 +130,7 @@ public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface s turnController = new PIDController(sd.getConst("TurnkP", 1), sd.getConst("TurnkI", 0), sd.getConst("TurnkD", 0), kf, ahrs, this); // tim = new Timer(); - SmartDashboard.putData("Turn PID", turnController); + sd.putData("Turn PID", turnController); } /** @@ -203,8 +205,8 @@ protected boolean isFinished() { protected void end() { turnController.disable(); System.out.println("end"); - SmartDashboard.putNumber("Turn PID Result", turnController.get()); - SmartDashboard.putNumber("Turn PID Error", turnController.getError()); + sd.putNumber("Turn PID Result", turnController.get()); + sd.putNumber("Turn PID Error", turnController.getError()); // turnController.free(); AutoUtils.position.changeRot(dt.getAHRSAngle()); From f4536514b2026f4040ac35d6bc24473e907a9ee0 Mon Sep 17 00:00:00 2001 From: lhmcgann Date: Mon, 19 Feb 2018 21:08:50 -0800 Subject: [PATCH 12/14] testing auto changes relative vs absolute = problems moved math in PIDMove and PIDTurn to initialize instead of in the constructor added try catch for manipulator in DefaultIntake command (Daniel P, Kevin W, Laura M, Dean B) --- Robot2018/lib/User_Libraries.properties | 2 +- .../org/usfirst/frc/team199/Robot2018/OI.java | 2 +- .../usfirst/frc/team199/Robot2018/Robot.java | 15 +- .../Robot2018/autonomous/AutoUtils.java | 7 +- .../autonomous/scriptupload/scriptupload.py | 3 + .../Robot2018/commands/AutoMoveTo.java | 9 +- .../Robot2018/commands/DefaultIntake.java | 14 +- .../team199/Robot2018/commands/PIDMove.java | 17 +- .../team199/Robot2018/commands/PIDTurn.java | 42 +++- .../team199/Robot2018/commands/RunScript.java | 8 +- shuffleboard.json | 215 ++++++++++++++++-- 11 files changed, 279 insertions(+), 55 deletions(-) diff --git a/Robot2018/lib/User_Libraries.properties b/Robot2018/lib/User_Libraries.properties index 1ee5cb2..997850e 100644 --- a/Robot2018/lib/User_Libraries.properties +++ b/Robot2018/lib/User_Libraries.properties @@ -1,2 +1,2 @@ -#Sun Feb 18 17:40:43 PST 2018 +#Mon Feb 19 20:57:02 PST 2018 C\:\\Users\\dean\\git\\RobotCode2018\\Robot2018\\lib\\libpathfinderjava.so=4087ee38c338446b8305f67716260df4 diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java index 1d7b7b5..69c6a64 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java @@ -86,7 +86,7 @@ public OI() { MoveLiftUpButton.whileHeld(new RunLift(Robot.lift, true)); MoveLiftDownButton.whileHeld(new RunLift(Robot.lift, false)); - // manipulator = new Joystick(2); + manipulator = new Joystick(2); // closeIntake = new JoystickButton(manipulator, getButton("Close Intake // Button", 1)); // closeIntake.whenPressed(new CloseIntake()); diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java index 76b8413..57c03fd 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java @@ -12,8 +12,8 @@ import java.util.Map; import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils; +import org.usfirst.frc.team199.Robot2018.autonomous.Position; import org.usfirst.frc.team199.Robot2018.commands.Autonomous; -import org.usfirst.frc.team199.Robot2018.commands.Autonomous.Position; import org.usfirst.frc.team199.Robot2018.commands.Autonomous.Strategy; import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear; import org.usfirst.frc.team199.Robot2018.subsystems.Climber; @@ -22,7 +22,6 @@ import org.usfirst.frc.team199.Robot2018.subsystems.IntakeEject; import org.usfirst.frc.team199.Robot2018.subsystems.Lift; -import edu.wpi.first.wpilibj.CameraServer; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.IterativeRobot; import edu.wpi.first.wpilibj.PIDController; @@ -54,7 +53,7 @@ public class Robot extends IterativeRobot { public static Map> autoScripts; Command autonomousCommand; - SendableChooser posChooser = new SendableChooser(); + SendableChooser posChooser = new SendableChooser(); Map> stratChoosers = new HashMap>(); String[] fmsPossibilities = { "LL", "LR", "RL", "RR" }; @@ -118,7 +117,7 @@ public void robotInit() { oi = new OI(); // auto position chooser - for (Position p : Position.values()) { + for (Autonomous.Position p : Autonomous.Position.values()) { posChooser.addObject(p.getSDName(), p); } SmartDashboard.putData("Starting Position", posChooser); @@ -140,8 +139,8 @@ public void robotInit() { autoScripts = AutoUtils.parseScriptFile(Preferences.getInstance().getString("autoscripts", "")); listen = new Listener(); - CameraServer.getInstance().startAutomaticCapture(0); - CameraServer.getInstance().startAutomaticCapture(1); + // CameraServer.getInstance().startAutomaticCapture(0); + // CameraServer.getInstance().startAutomaticCapture(1); } /** @@ -166,9 +165,11 @@ public void disabledPeriodic() { */ @Override public void autonomousInit() { + dt.resetAHRS(); + AutoUtils.position = new Position(0, 0, 0); Scheduler.getInstance().add(new ShiftLowGear()); String fmsInput = DriverStation.getInstance().getGameSpecificMessage(); - Position startPos = posChooser.getSelected(); + Autonomous.Position startPos = posChooser.getSelected(); double autoDelay = SmartDashboard.getNumber("Auto Delay", 0); Map strategies = new HashMap(); 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 3d16272..337ee6b 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java @@ -6,7 +6,7 @@ public class AutoUtils { - public static Position position = new Position(0, 0, 0); + public static Position position; /** * Parses the inputted script file into a map of scripts @@ -25,6 +25,7 @@ public static Map> parseScriptFile(String scriptFile String currScriptName = ""; int count = 0; + int errorCount = 0; for (String line : lines) { count++; // remove comments @@ -75,6 +76,8 @@ public static Map> parseScriptFile(String scriptFile if (isValidCommand(instruction, args, count)) { String[] command = { instruction, args }; currScript.add(command); + } else { + errorCount++; } } @@ -86,6 +89,8 @@ public static Map> parseScriptFile(String scriptFile // remove the stray one in the beginning autoScripts.remove(""); + System.out.println("[INFO] Successfuly parsed " + count + " lines with " + errorCount + " errors."); + return autoScripts; } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/scriptupload/scriptupload.py b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/scriptupload/scriptupload.py index bedc85e..948a80f 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/scriptupload/scriptupload.py +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/scriptupload/scriptupload.py @@ -1,6 +1,7 @@ #nt setup from networktables import NetworkTables import __future__ +import time NetworkTables.initialize(server='10.1.99.2') prefs = NetworkTables.getTable("Preferences") @@ -30,6 +31,8 @@ prefs.putString("autoscripts", oneline) print("Uploading %s as a String[] to key \"autoscripts\"" % filename) +time.sleep(0.1) + #checks if the key has been filled tester = "" #a variable to check if autoscripts is None 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 76e0333..a56ee3e 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java @@ -17,17 +17,18 @@ */ public class AutoMoveTo extends CommandGroup { - public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource pidMoveSrc) { + public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource pidMoveSrc, + PIDSource pidTurnSource) { // requires(Drivetrain); double rotation; double[] point = { 0, 0 }; for (String arg : args) { if (AutoUtils.isDouble(arg)) { rotation = Double.valueOf(arg); - addSequential(new PIDTurn(rotation, dt, sd, pidMoveSrc, true)); + addSequential(new PIDTurn(rotation, dt, sd, pidTurnSource, true)); } else if (AutoUtils.isPoint(arg)) { point = AutoUtils.parsePoint(arg); - addSequential(new PIDTurn(point, dt, sd, pidMoveSrc)); + addSequential(new PIDTurn(point, dt, sd, pidTurnSource)); addSequential(new PIDMove(point, dt, sd, pidMoveSrc)); } else { throw new IllegalArgumentException(); @@ -36,6 +37,6 @@ public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface } public AutoMoveTo(String[] args) { - this(args, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg()); + this(args, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg(), Robot.dt.getGyro()); } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/DefaultIntake.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/DefaultIntake.java index b6fa578..c7e7372 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/DefaultIntake.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/DefaultIntake.java @@ -9,6 +9,8 @@ */ public class DefaultIntake extends Command { + private boolean manipulatorPluggedIn = true; + public DefaultIntake() { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); @@ -17,13 +19,21 @@ public DefaultIntake() { // Called just before this Command runs the first time protected void initialize() { + try { + Robot.oi.manipulator.getRawAxis(1); + } catch (NullPointerException e) { + System.err.println("[ERROR] Manipulator not plugged in."); + manipulatorPluggedIn = false; + } } // Called repeatedly when this Command is scheduled to run protected void execute() { // 1 and 5 represent the axes' index in driver station - Robot.intakeEject.runLeftIntake(Robot.oi.manipulator.getRawAxis(1)); - Robot.intakeEject.runRightIntake(Robot.oi.manipulator.getRawAxis(5)); + if (manipulatorPluggedIn) { + Robot.intakeEject.runLeftIntake(Robot.oi.manipulator.getRawAxis(1)); + Robot.intakeEject.runRightIntake(Robot.oi.manipulator.getRawAxis(5)); + } } // Make this return true when this Command no longer needs to run execute() 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 505c1a4..65f864d 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java @@ -21,6 +21,8 @@ public class PIDMove extends Command implements PIDOutput { private PIDController moveController; private PIDSource avg; private SmartDashboardInterface sd; + private double pointX; + private double pointY; /** * Constructs this command with a new PIDController. Sets all of the @@ -71,12 +73,9 @@ public PIDMove(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, * 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(); + pointX = point[0]; + pointY = point[1]; - double dist = Math.sqrt(dx * dx + dy * dy); // pythagorean theorem to find distance - - this.target = dist; this.dt = dt; this.avg = avg; this.sd = sd; @@ -86,6 +85,7 @@ public PIDMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface s 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); + sd.putData("Move PID", moveController); } /** @@ -94,6 +94,12 @@ public PIDMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface s */ @Override public void initialize() { + double dx = pointX - AutoUtils.position.getX(); + double dy = pointY - AutoUtils.position.getY(); + + double dist = Math.sqrt(dx * dx + dy * dy); // pythagorean theorem to find distance + this.target = dist; + dt.resetDistEncs(); moveController.disable(); // input is in inches @@ -102,6 +108,7 @@ public void initialize() { moveController.setOutputRange(-1.0, 1.0); moveController.setContinuous(false); moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 0.1)); + System.out.println("move target = " + target); moveController.setSetpoint(target); moveController.enable(); 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 88084cc..c3a9d9d 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java @@ -25,6 +25,10 @@ public class PIDTurn extends Command implements PIDOutput { private Timer tim; private double lastTime; private SmartDashboardInterface sd; + private double pointX; + private double pointY; + private boolean turnToPoint; + private boolean absoluteRotation; /** * Constructs this command with a new PIDController. Sets all of the @@ -52,11 +56,9 @@ public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, this.ahrs = ahrs; this.sd = sd; - if (absolute) { - target = targ - AutoUtils.position.getRot(); - } else { - target = targ; - } + turnToPoint = false; + target = targ; + absoluteRotation = absolute; if (Robot.dt != null) { requires(Robot.dt); @@ -113,12 +115,12 @@ public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd, public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource ahrs) { this.dt = dt; this.ahrs = ahrs; + this.sd = sd; - double dx = point[0] - AutoUtils.position.getX(); - double dy = point[1] - AutoUtils.position.getY(); + pointX = point[0]; + pointY = point[1]; - double absTurn = Math.toDegrees(Math.atan2(dy, dx)); - target = absTurn - AutoUtils.position.getRot(); + turnToPoint = true; if (Robot.dt != null) { requires(Robot.dt); @@ -139,6 +141,25 @@ public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface s */ @Override protected void initialize() { + + if (!turnToPoint) { + if (absoluteRotation) { + target -= AutoUtils.position.getRot(); + } + } else { + double dx = pointX - AutoUtils.position.getX(); + double dy = pointY - AutoUtils.position.getY(); + + System.out.println("x = " + dx + ", y = " + dy); + + double absTurn = Math.toDegrees(Math.atan2(dx, dy)); + target = absTurn - AutoUtils.position.getRot(); + System.out.println("position rotation = " + AutoUtils.position.getRot()); + System.out.println("target = " + target); + } + + System.out.println("Turn to point: " + turnToPoint); + turnController.disable(); // dt.enableVelocityPIDs(); System.out.println("initialize2s"); @@ -155,6 +176,7 @@ protected void initialize() { while (Math.abs(newSetPoint) > 180) { newSetPoint = newSetPoint - Math.signum(newSetPoint) * 360; } + System.out.println("set point = " + newSetPoint); turnController.setSetpoint(newSetPoint); turnController.enable(); @@ -209,7 +231,7 @@ protected void end() { sd.putNumber("Turn PID Error", turnController.getError()); // turnController.free(); - AutoUtils.position.changeRot(dt.getAHRSAngle()); + AutoUtils.position.setRot(dt.getAHRSAngle()); } /** 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 bef69bf..e4946c4 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunScript.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunScript.java @@ -14,7 +14,13 @@ public class RunScript extends CommandGroup { public RunScript(String scriptName) { - ArrayList script = Robot.autoScripts.getOrDefault(scriptName, new ArrayList()); + ArrayList script; + if (Robot.autoScripts.containsKey(scriptName)) { + script = Robot.autoScripts.get(scriptName); + } else { + System.err.println("[ERROR] auto scripts file does not contain script `" + scriptName + "`."); + return; + } outerloop: for (String[] cmd : script) { String cmdName = cmd[0]; diff --git a/shuffleboard.json b/shuffleboard.json index 978afee..56522f3 100644 --- a/shuffleboard.json +++ b/shuffleboard.json @@ -1,5 +1,5 @@ { - "dividerPosition": 0.45185185185185184, + "dividerPosition": 0.45234604105571846, "tabPane": [ { "title": "PID Testing", @@ -265,17 +265,6 @@ "_title": "SmartDashboard/Const/Move Targ" } }, - "0,0": { - "size": [ - 1, - 2 - ], - "content": { - "_type": "PID Controller", - "_source0": "network_table:///SmartDashboard/Move PID", - "_title": "SmartDashboard/Move PID" - } - }, "3,0": { "size": [ 2, @@ -402,6 +391,17 @@ "_source0": "network_table:///SmartDashboard/Const/MoveTolerance", "_title": "SmartDashboard/Const/MoveTolerance" } + }, + "0,0": { + "size": [ + 1, + 2 + ], + "content": { + "_type": "PID Controller", + "_source0": "network_table:///SmartDashboard/Move PID", + "_title": "SmartDashboard/Move PID" + } } } } @@ -416,7 +416,7 @@ "hgap": 16.0, "vgap": 16.0, "tiles": { - "1,0": { + "2,0": { "size": [ 2, 1 @@ -427,7 +427,7 @@ "_title": "LiveWindow/Ungrouped/Scheduler" } }, - "1,3": { + "3,1": { "size": [ 1, 1 @@ -438,7 +438,7 @@ "_title": "SmartDashboard/Turn PID Error" } }, - "0,3": { + "4,1": { "size": [ 1, 1 @@ -449,7 +449,7 @@ "_title": "SmartDashboard/Turn PID Output" } }, - "1,2": { + "2,1": { "size": [ 1, 1 @@ -460,9 +460,9 @@ "_title": "SmartDashboard/Turn PID Result" } }, - "2,1": { + "0,2": { "size": [ - 1, + 2, 1 ], "content": { @@ -473,7 +473,7 @@ }, "0,0": { "size": [ - 1, + 2, 2 ], "content": { @@ -482,7 +482,7 @@ "_title": "SmartDashboard/Turn PID" } }, - "2,2": { + "4,3": { "size": [ 1, 1 @@ -493,7 +493,7 @@ "_title": "SmartDashboard/Left VPID Error" } }, - "2,3": { + "4,2": { "size": [ 1, 1 @@ -504,7 +504,7 @@ "_title": "SmartDashboard/Right VPID Error" } }, - "0,2": { + "4,0": { "size": [ 1, 1 @@ -515,7 +515,7 @@ "_title": "SmartDashboard/Const/Turn Targ" } }, - "1,1": { + "3,3": { "size": [ 1, 1 @@ -525,6 +525,175 @@ "_source0": "network_table:///SmartDashboard/Const/TurnTolerance", "_title": "SmartDashboard/Const/TurnTolerance" } + }, + "2,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///Preferences/Const/Output", + "_title": "Preferences/Const/Output" + } + } + } + } + }, + { + "title": "Tab 5", + "autoPopulate": false, + "autoPopulatePrefix": "", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "tiles": { + "0,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///Preferences/Const/TurnkD", + "_title": "Preferences/Const/TurnkD" + } + }, + "0,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///Preferences/Const/TurnkI", + "_title": "Preferences/Const/TurnkI" + } + }, + "0,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///Preferences/Const/TurnkP", + "_title": "Preferences/Const/TurnkP" + } + }, + "1,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///Preferences/Const/Output", + "_title": "Preferences/Const/Output" + } + } + } + } + }, + { + "title": "Autonomous", + "autoPopulate": false, + "autoPopulatePrefix": "", + "widgetPane": { + "gridSize": 128.0, + "showGrid": true, + "hgap": 16.0, + "vgap": 16.0, + "tiles": { + "0,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "ComboBox Chooser", + "_source0": "network_table:///SmartDashboard/Starting Position", + "_title": "SmartDashboard/Starting Position" + } + }, + "0,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "ComboBox Chooser", + "_source0": "network_table:///SmartDashboard/LL", + "_title": "SmartDashboard/LL" + } + }, + "1,1": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "ComboBox Chooser", + "_source0": "network_table:///SmartDashboard/LR", + "_title": "SmartDashboard/LR" + } + }, + "0,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "ComboBox Chooser", + "_source0": "network_table:///SmartDashboard/RL", + "_title": "SmartDashboard/RL" + } + }, + "1,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "ComboBox Chooser", + "_source0": "network_table:///SmartDashboard/RR", + "_title": "SmartDashboard/RR" + } + }, + "1,0": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///SmartDashboard/Auto Delay", + "_title": "SmartDashboard/Auto Delay" + } + }, + "2,0": { + "size": [ + 3, + 2 + ], + "content": { + "_type": "Network Table Tree", + "_source0": "network_table:///LiveWindow/Ungrouped/Scheduler", + "_title": "LiveWindow/Ungrouped/Scheduler" + } + }, + "2,2": { + "size": [ + 1, + 1 + ], + "content": { + "_type": "Text View", + "_source0": "network_table:///Preferences/Const/MoveTolerance", + "_title": "Preferences/Const/MoveTolerance" + } } } } From e17a2052af57441f46037168216fcca1d134a881 Mon Sep 17 00:00:00 2001 From: lhmcgann Date: Tue, 20 Feb 2018 09:55:05 -0800 Subject: [PATCH 13/14] fixed PIDMove and other tiny random things PIDMove was calculating as if numbers were normal angles when in fact they are bearings so we switched x and y --- Robot2018/lib/User_Libraries.properties | 2 +- .../frc/team199/Robot2018/autonomous/AutoUtils.java | 2 +- .../usfirst/frc/team199/Robot2018/commands/PIDMove.java | 7 +++++-- .../usfirst/frc/team199/Robot2018/commands/PIDTurn.java | 5 +++-- 4 files changed, 10 insertions(+), 6 deletions(-) diff --git a/Robot2018/lib/User_Libraries.properties b/Robot2018/lib/User_Libraries.properties index 997850e..80d03de 100644 --- a/Robot2018/lib/User_Libraries.properties +++ b/Robot2018/lib/User_Libraries.properties @@ -1,2 +1,2 @@ -#Mon Feb 19 20:57:02 PST 2018 +#Tue Feb 20 09:44:13 PST 2018 C\:\\Users\\dean\\git\\RobotCode2018\\Robot2018\\lib\\libpathfinderjava.so=4087ee38c338446b8305f67716260df4 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 337ee6b..3d74899 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/AutoUtils.java @@ -249,7 +249,7 @@ public static double[] parsePoint(String cmdArgs) { double[] point = new double[2]; String parentheseless; String[] pointparts; - if (AutoUtils.isPoint(cmdArgs)) { + if (isPoint(cmdArgs)) { parentheseless = cmdArgs.substring(1, cmdArgs.length() - 1); pointparts = parentheseless.split(","); try { 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 65f864d..bd3cd8a 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java @@ -153,10 +153,13 @@ protected void end() { double angle = Math.toRadians(AutoUtils.position.getRot()); double dist = avg.pidGet(); - double x = Math.cos(angle) * dist; - double y = Math.sin(angle) * dist; + // x and y are switched because we are using bearings + double y = Math.cos(angle) * dist; + double x = Math.sin(angle) * dist; AutoUtils.position.changeX(x); AutoUtils.position.changeY(y); + + AutoUtils.position.setRot(dt.getAHRSAngle()); } /** 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 c3a9d9d..19c27de 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDTurn.java @@ -152,10 +152,11 @@ protected void initialize() { System.out.println("x = " + dx + ", y = " + dy); + // x and y are switched because we are using bearings double absTurn = Math.toDegrees(Math.atan2(dx, dy)); target = absTurn - AutoUtils.position.getRot(); - System.out.println("position rotation = " + AutoUtils.position.getRot()); - System.out.println("target = " + target); + System.out.println("current bearing = " + AutoUtils.position.getRot()); + System.out.println("target bearing = " + target); } System.out.println("Turn to point: " + turnToPoint); From 7cc1111f74c5e9293904c02fae9bc7c2c67a19ef Mon Sep 17 00:00:00 2001 From: DriverStationComputer Date: Tue, 20 Feb 2018 10:37:31 -0800 Subject: [PATCH 14/14] fix AutoMoveTo test --- .../org/usfirst/frc/team199/Robot2018/AutoMoveToTest.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Robot2018/test/org/usfirst/frc/team199/Robot2018/AutoMoveToTest.java b/Robot2018/test/org/usfirst/frc/team199/Robot2018/AutoMoveToTest.java index 27d2560..ed6b208 100644 --- a/Robot2018/test/org/usfirst/frc/team199/Robot2018/AutoMoveToTest.java +++ b/Robot2018/test/org/usfirst/frc/team199/Robot2018/AutoMoveToTest.java @@ -102,7 +102,7 @@ void testForwardAndRight() { SmartDashboardInterface sd = mock(SmartDashboardInterface.class); PIDSource pidMoveSrc = mock(PIDSource.class); - AutoMoveTo testAMT = new AutoMoveTo(args, dt, sd, pidMoveSrc); + AutoMoveTo testAMT = new AutoMoveTo(args, dt, sd, pidMoveSrc, pidGyroSrc); assertEquals(90, AutoUtils.position.getRot()); assertEquals(12, AutoUtils.position.getX()); @@ -124,7 +124,7 @@ void testForward() { SmartDashboardInterface sd = mock(SmartDashboardInterface.class); PIDSource pidMoveSrc = mock(PIDSource.class); - AutoMoveTo testAMT = new AutoMoveTo(args, dt, sd, pidMoveSrc); + AutoMoveTo testAMT = new AutoMoveTo(args, dt, sd, pidMoveSrc, pidGyroSrc); assertEquals(0, AutoUtils.position.getRot()); assertEquals(0, AutoUtils.position.getX());