From e285976c5211c650ec2cb34a886815026acc1b5d Mon Sep 17 00:00:00 2001 From: Kevin Wamg <20215378+kevinzwang@users.noreply.github.com> Date: Fri, 9 Mar 2018 20:46:01 -0800 Subject: [PATCH 1/3] add accounting for center of rotation --- .../Robot2018/commands/AutoMoveTo.java | 33 ++++++++++++------- 1 file changed, 22 insertions(+), 11 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 a56ee3e..7f52d4e 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java @@ -20,19 +20,30 @@ public class AutoMoveTo extends CommandGroup { public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource pidMoveSrc, PIDSource pidTurnSource) { // requires(Drivetrain); + double centerOfRotError = 0; double rotation; double[] point = { 0, 0 }; - for (String arg : args) { - if (AutoUtils.isDouble(arg)) { - rotation = Double.valueOf(arg); - addSequential(new PIDTurn(rotation, dt, sd, pidTurnSource, true)); - } else if (AutoUtils.isPoint(arg)) { - point = AutoUtils.parsePoint(arg); - addSequential(new PIDTurn(point, dt, sd, pidTurnSource)); - addSequential(new PIDMove(point, dt, sd, pidMoveSrc)); - } else { - throw new IllegalArgumentException(); - } + + for (int i = 0; i < args.length - 1; i++) { + point = AutoUtils.parsePoint(args[i]); + addSequential(new PIDTurn(point, dt, sd, pidTurnSource)); + + double dist = Math.sqrt(point[0]*point[0] + point[1]*point[1]) + centerOfRotError; // TODO: subtract rotation error + addSequential(new PIDMove(dist, dt, sd, pidMoveSrc)); + } + + String lastArg = args[args.length - 1]; + // if the last argument is a point, no need for extra positioning at the end + if (AutoUtils.isPoint(lastArg)) { + point = AutoUtils.parsePoint(lastArg); + addSequential(new PIDTurn(point, dt, sd, pidTurnSource)); + addSequential(new PIDMove(point, dt, sd, pidMoveSrc)); + } else if (AutoUtils.isDouble(lastArg)) { // move a little after turn to end up in the correct position + rotation = Double.valueOf(lastArg); + addSequential(new PIDTurn(rotation, dt, sd, pidTurnSource, true)); + addSequential(new PIDTurn(centerOfRotError, dt, sd, pidTurnSource)); + } else { + throw new IllegalArgumentException(); } } From 10f905df122d13542db845baa53332625f1391a0 Mon Sep 17 00:00:00 2001 From: Kevin Wamg <20215378+kevinzwang@users.noreply.github.com> Date: Fri, 9 Mar 2018 21:37:17 -0800 Subject: [PATCH 2/3] add input in NT for setting center of rotation things should be tested on the bot now, also AutoMoveToTest fails but I don't know if its because of the changes in this commit --- .../src/org/usfirst/frc/team199/Robot2018/Robot.java | 6 +++--- .../frc/team199/Robot2018/autonomous/State.java | 8 +++++++- .../frc/team199/Robot2018/commands/AutoMoveTo.java | 11 +++++------ .../frc/team199/Robot2018/commands/RunScript.java | 2 +- .../usfirst/frc/team199/Robot2018/AutoMoveToTest.java | 5 +++-- 5 files changed, 19 insertions(+), 13 deletions(-) diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java index 045fda7..88b976e 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java @@ -184,7 +184,7 @@ public void disabledPeriodic() { @Override public void autonomousInit() { dt.resetAHRS(); - AutoUtils.state = new State(0, 0, 0); + AutoUtils.state = new State(0, 0, 0, SmartDashboard.getNumber("Center of Rotation", 0)); Scheduler.getInstance().add(new ShiftLowGear()); Scheduler.getInstance().add(new CloseIntake()); String fmsInput = DriverStation.getInstance().getGameSpecificMessage(); @@ -214,7 +214,7 @@ public void autonomousPeriodic() { public void teleopInit() { System.out.println("In teleopInit()"); dt.resetAHRS(); - AutoUtils.state = new State(0, 0, 0); + AutoUtils.state = new State(0, 0, 0, SmartDashboard.getNumber("Center of Rotation", 0)); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove @@ -258,7 +258,7 @@ public void teleopPeriodic() { public void testInit() { System.out.println("In testInit()"); dt.resetAHRS(); - AutoUtils.state = new State(0, 0, 0); + AutoUtils.state = new State(0, 0, 0, SmartDashboard.getNumber("Center of Rotation", 0)); } /** diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/State.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/State.java index e372f23..a505624 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/State.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/autonomous/State.java @@ -4,11 +4,13 @@ public class State { private double currX; private double currY; private double currRotation; + private double centerOfRot; - public State(double x, double y, double rot) { + public State(double x, double y, double rot, double centerOfRot) { currX = x; currY = y; currRotation = rot; + this.centerOfRot = centerOfRot; } /* @@ -49,4 +51,8 @@ public void changeY(double y) { public void changeRot(double rot) { currRotation += rot; } + + public double getCenterOfRot() { + return centerOfRot; + } } 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 7f52d4e..2641515 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java @@ -17,10 +17,9 @@ */ public class AutoMoveTo extends CommandGroup { - public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource pidMoveSrc, + public AutoMoveTo(String[] args, double centerOfRot, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource pidMoveSrc, PIDSource pidTurnSource) { // requires(Drivetrain); - double centerOfRotError = 0; double rotation; double[] point = { 0, 0 }; @@ -28,7 +27,7 @@ public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface point = AutoUtils.parsePoint(args[i]); addSequential(new PIDTurn(point, dt, sd, pidTurnSource)); - double dist = Math.sqrt(point[0]*point[0] + point[1]*point[1]) + centerOfRotError; // TODO: subtract rotation error + double dist = Math.sqrt(point[0]*point[0] + point[1]*point[1]) + centerOfRot; // TODO: subtract rotation error addSequential(new PIDMove(dist, dt, sd, pidMoveSrc)); } @@ -41,13 +40,13 @@ public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface } else if (AutoUtils.isDouble(lastArg)) { // move a little after turn to end up in the correct position rotation = Double.valueOf(lastArg); addSequential(new PIDTurn(rotation, dt, sd, pidTurnSource, true)); - addSequential(new PIDTurn(centerOfRotError, dt, sd, pidTurnSource)); + addSequential(new PIDTurn(centerOfRot, dt, sd, pidTurnSource)); } else { throw new IllegalArgumentException(); } } - public AutoMoveTo(String[] args) { - this(args, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg(), Robot.dt.getGyro()); + public AutoMoveTo(String[] args, double centerOfRot) { + this(args, centerOfRot, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg(), Robot.dt.getGyro()); } } diff --git a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunScript.java b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunScript.java index e4946c4..1487562 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunScript.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunScript.java @@ -28,7 +28,7 @@ public RunScript(String scriptName) { switch (cmdName) { case "moveto": - addSequential(new AutoMoveTo(cmdArgs.split(" "))); + addSequential(new AutoMoveTo(cmdArgs.split(" "), AutoUtils.state.getCenterOfRot())); break; case "turn": if (AutoUtils.isPoint(cmdArgs)) { diff --git a/Robot2018/test/org/usfirst/frc/team199/Robot2018/AutoMoveToTest.java b/Robot2018/test/org/usfirst/frc/team199/Robot2018/AutoMoveToTest.java index 51fafe3..d6f54ea 100644 --- a/Robot2018/test/org/usfirst/frc/team199/Robot2018/AutoMoveToTest.java +++ b/Robot2018/test/org/usfirst/frc/team199/Robot2018/AutoMoveToTest.java @@ -8,6 +8,7 @@ import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; import org.usfirst.frc.team199.Robot2018.autonomous.AutoUtils; +import org.usfirst.frc.team199.Robot2018.autonomous.State; import org.usfirst.frc.team199.Robot2018.commands.AutoMoveTo; import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface; @@ -102,7 +103,7 @@ void testForwardAndRight() { SmartDashboardInterface sd = mock(SmartDashboardInterface.class); PIDSource pidMoveSrc = mock(PIDSource.class); - AutoMoveTo testAMT = new AutoMoveTo(args, dt, sd, pidMoveSrc, pidGyroSrc); + AutoMoveTo testAMT = new AutoMoveTo(args, 0, dt, sd, pidMoveSrc, pidGyroSrc); assertEquals(90, AutoUtils.state.getRot()); assertEquals(12, AutoUtils.state.getX()); @@ -124,7 +125,7 @@ void testForward() { SmartDashboardInterface sd = mock(SmartDashboardInterface.class); PIDSource pidMoveSrc = mock(PIDSource.class); - AutoMoveTo testAMT = new AutoMoveTo(args, dt, sd, pidMoveSrc, pidGyroSrc); + AutoMoveTo testAMT = new AutoMoveTo(args, 0, dt, sd, pidMoveSrc, pidGyroSrc); assertEquals(0, AutoUtils.state.getRot()); assertEquals(0, AutoUtils.state.getX()); From 067bfd82fc268f63c92929d48db2d64fc5a5845b Mon Sep 17 00:00:00 2001 From: Kevin Wamg <20215378+kevinzwang@users.noreply.github.com> Date: Wed, 14 Mar 2018 16:33:34 -0700 Subject: [PATCH 3/3] fix copy paste typo it's PIDMove in the end not PIDMove, also gets center of rotation from AutoUtils.state instead of passing it in --- .../frc/team199/Robot2018/commands/AutoMoveTo.java | 10 +++++----- .../frc/team199/Robot2018/commands/RunScript.java | 2 +- .../usfirst/frc/team199/Robot2018/AutoMoveToTest.java | 4 ++-- 3 files changed, 8 insertions(+), 8 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 d775b86..67d441e 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java @@ -17,7 +17,7 @@ */ public class AutoMoveTo extends CommandGroup { - public AutoMoveTo(String[] args, double centerOfRot, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource pidMoveSrc, + public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface sd, PIDSource pidMoveSrc, PIDSource pidTurnSource) { // requires(Drivetrain); double rotation; @@ -27,7 +27,7 @@ public AutoMoveTo(String[] args, double centerOfRot, DrivetrainInterface dt, Sma point = AutoUtils.parsePoint(args[i]); addSequential(new PIDTurn(point, dt, sd, pidTurnSource)); - double dist = Math.sqrt(point[0]*point[0] + point[1]*point[1]) + centerOfRot; // TODO: subtract rotation error + double dist = Math.sqrt(point[0]*point[0] + point[1]*point[1]) + AutoUtils.state.getCenterOfRot(); // TODO: subtract rotation error addSequential(new PIDMove(dist, dt, sd, pidMoveSrc)); } @@ -40,13 +40,13 @@ public AutoMoveTo(String[] args, double centerOfRot, DrivetrainInterface dt, Sma } else if (AutoUtils.isDouble(lastArg)) { // move a little after turn to end up in the correct position rotation = Double.valueOf(lastArg); addSequential(new PIDTurn(rotation, dt, sd, pidTurnSource, true)); - addSequential(new PIDTurn(centerOfRot, dt, sd, pidTurnSource)); + addSequential(new PIDMove(AutoUtils.state.getCenterOfRot(), dt, sd, pidTurnSource)); } else { throw new IllegalArgumentException(); } } - public AutoMoveTo(String[] args, double centerOfRot) { - this(args, centerOfRot, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg(), Robot.dt.getGyro()); + public AutoMoveTo(String[] args) { + this(args, Robot.dt, Robot.sd, Robot.dt.getDistEncAvg(), Robot.dt.getGyro()); } } \ No newline at end of file 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 1487562..e4946c4 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunScript.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/RunScript.java @@ -28,7 +28,7 @@ public RunScript(String scriptName) { switch (cmdName) { case "moveto": - addSequential(new AutoMoveTo(cmdArgs.split(" "), AutoUtils.state.getCenterOfRot())); + addSequential(new AutoMoveTo(cmdArgs.split(" "))); break; case "turn": if (AutoUtils.isPoint(cmdArgs)) { diff --git a/Robot2018/test/org/usfirst/frc/team199/Robot2018/AutoMoveToTest.java b/Robot2018/test/org/usfirst/frc/team199/Robot2018/AutoMoveToTest.java index d6f54ea..ddaeb9f 100644 --- a/Robot2018/test/org/usfirst/frc/team199/Robot2018/AutoMoveToTest.java +++ b/Robot2018/test/org/usfirst/frc/team199/Robot2018/AutoMoveToTest.java @@ -103,7 +103,7 @@ void testForwardAndRight() { SmartDashboardInterface sd = mock(SmartDashboardInterface.class); PIDSource pidMoveSrc = mock(PIDSource.class); - AutoMoveTo testAMT = new AutoMoveTo(args, 0, dt, sd, pidMoveSrc, pidGyroSrc); + AutoMoveTo testAMT = new AutoMoveTo(args, dt, sd, pidMoveSrc, pidGyroSrc); assertEquals(90, AutoUtils.state.getRot()); assertEquals(12, AutoUtils.state.getX()); @@ -125,7 +125,7 @@ void testForward() { SmartDashboardInterface sd = mock(SmartDashboardInterface.class); PIDSource pidMoveSrc = mock(PIDSource.class); - AutoMoveTo testAMT = new AutoMoveTo(args, 0, dt, sd, pidMoveSrc, pidGyroSrc); + AutoMoveTo testAMT = new AutoMoveTo(args, dt, sd, pidMoveSrc, pidGyroSrc); assertEquals(0, AutoUtils.state.getRot()); assertEquals(0, AutoUtils.state.getX());