Center of rotation

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));
 	}
 
 	/**

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;
+	}
 }

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();
 		}
 	}
 

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;