diff options
Diffstat (limited to 'src/org')
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/Autonomous.java | 122 |
1 files changed, 89 insertions, 33 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java index 51c2061..d012a98 100644 --- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java +++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java @@ -28,51 +28,107 @@ package org.usfirst.frc.team4272.robot2017; public class Autonomous { - private final HwRobot robot; - - public Autonomous(HwRobot robot) { - this.robot = robot; - robot.lDriveE.reset(); - robot.rDriveE.reset(); + private static interface Command { + public boolean execute(Control c); } - private boolean driveDistance(Control c, double speed, double dist) { - double lDist = robot.lDriveE.getDistance(); - double rDist = robot.rDriveE.getDistance(); + private static class DriveDistance implements Command { + private static final double pctTaper = 1.0/3; + private static final double pctTolerance = 0.005; + private static final double pctThrottle = 0.2; + private static final double distFinished = 0.1; - double thresh = dist * 2.0/3.0; - double max = dist * 1.05; - - if (lDist < thresh) { - c.lDrive = speed; - } else if (lDist < max) { - c.lDrive = speed*Math.sqrt((dist-lDist)/(dist-thresh)); - } else { - c.lDrive = 0; + private final HwRobot robot; + private final double lSpeed, lDistTarget; + private final double rSpeed, rDistTarget; + public DriveDistance(HwRobot robot, + double lSpeed, double lDistTarget, + double rSpeed, double rDistTarget) { + this.robot = robot; + this.lSpeed = lSpeed; + this.lDistTarget = lDistTarget; + this.rSpeed = rSpeed; + this.rDistTarget = rDistTarget; } - - if (rDist < thresh) { - c.rDrive = speed; - } else if (rDist < max) { - c.rDrive = speed*Math.sqrt((dist-rDist)/(dist-thresh)); - } else { - c.rDrive = 0; - return true; + public DriveDistance(HwRobot robot, double speed, double target) { + this(robot, speed, target, speed, target); } + private boolean initialized = false; + public boolean execute(Control c) { + if (!initialized) { + robot.lDriveE.reset(); + robot.rDriveE.reset(); + initialized = true; + } + double lDistCurr = robot.lDriveE.getDistance(); + double rDistCurr = robot.rDriveE.getDistance(); + double lPct = lDistCurr/lDistTarget; + double rPct = rDistCurr/rDistTarget; - if (Math.abs(lDist - rDist) > 0.1) { - if (lDist > rDist) { - c.lDrive *= 0.2; - } else { - c.rDrive *= 0.2; + c.lDrive = Math.copySign(lSpeed, 1-lPct); + if (Math.abs(1-lPct) < pctTaper) { + c.lDrive *= Math.sqrt(Math.abs((1-lPct)/pctTaper)); } + + c.rDrive = Math.copySign(rSpeed, 1-rPct); + if (Math.abs(1-rPct) < pctTaper) { + c.rDrive *= Math.sqrt(Math.abs((1-lPct)/pctTaper)); + } + + if (Math.abs(lPct - rPct) > pctTolerance) { + if (lPct > rPct) { + c.lDrive *= pctThrottle; + } else { + c.rDrive *= pctThrottle; + } + } + return Math.abs(lDistCurr - lDistTarget) < distFinished && Math.abs(rDistCurr - rDistTarget) < distFinished; } - return false; + } + + private static final double axleWidth = 2; // in feet + private static Command driveDistance(HwRobot robot, double speed, double dist) { + return new DriveDistance(robot, speed, dist); + } + private static Command turnRadians(HwRobot robot, double speed, double rad) { + return new DriveDistance(robot, + speed, Math.copySign(rad*axleWidth, rad), + speed, Math.copySign(rad*axleWidth, -rad)); + } + private static Command turnDegrees(HwRobot robot, double speed, double deg) { + return turnRadians(robot, speed, deg * Math.PI/180); + } + + private final HwRobot robot; + private final Command[] commands; + private int step = 0; + + public Autonomous(HwRobot robot) { + this.robot = robot; + + // Center peg + /* + commands = new Command[]{ + driveDistance(robot, 0.3, 8.75), + }; + */ + + // Left peg + commands = new Command[]{ + driveDistance(robot, 0.3, 3.5), + turnDegrees(robot, 0.3, 60), + driveDistance(robot, 0.3, 8.3), + }; } public Control run(Control c) { c.gedOut = false; - driveDistance(c, 0.3, 8.75); + + if (step < commands.length) { + if (commands[step].execute(c)) + step++; + } + return c; } } |