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 14bb953..67d441e 100644 --- a/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java +++ b/Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/AutoMoveTo.java @@ -22,17 +22,27 @@ public AutoMoveTo(String[] args, DrivetrainInterface dt, SmartDashboardInterface // 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, 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]) + AutoUtils.state.getCenterOfRot(); // 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 PIDMove(AutoUtils.state.getCenterOfRot(), dt, sd, pidTurnSource)); + } else { + throw new IllegalArgumentException(); } } diff --git a/Robot2018/test/org/usfirst/frc/team199/Robot2018/AutoMoveToTest.java b/Robot2018/test/org/usfirst/frc/team199/Robot2018/AutoMoveToTest.java index 51fafe3..ddaeb9f 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;