diff options
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/Autonomous.java | 29 |
1 files changed, 16 insertions, 13 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java index 5ce3aa5..5fa7257 100644 --- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java +++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java @@ -42,14 +42,14 @@ public class Autonomous { } private static class DriveDistance implements Command { - private static final double pctTaper = 1.0/3; + private static final double pctTaper = 1.0/6; private static final double pctTolerance = 0.005; private final HwRobot robot; private final double pctThrottle; private final double lSpeed, lDistTarget; private final double rSpeed, rDistTarget; - private double lMult = 1 , rMult = 1; + private double lMin = 0, rMin = 0; public DriveDistance(HwRobot robot, double pctThrottle, double lSpeed, double lDistTarget, double rSpeed, double rDistTarget) { @@ -75,10 +75,9 @@ public class Autonomous { c.lDrive = lSpeed * Math.signum(lDistTarget) * Math.signum(1-lPct); if (Math.abs(1-lPct) < pctTaper) { c.lDrive *= Math.abs((1-lPct)/pctTaper); - if ( Math.abs(robot.lRate.pidGet()) < 0.05) { - lMult += 0.05; + if (Math.abs(robot.lRate.pidGet()) < 0.5) { + lMin = c.lDrive = Math.max(c.lDrive, lMin) * 1.05; } - c.lDrive *= lMult; if (c.lDrive > lSpeed) { c.lDrive = lSpeed; } @@ -87,10 +86,9 @@ public class Autonomous { c.rDrive = rSpeed * Math.signum(rDistTarget) * Math.signum(1-rPct); if (Math.abs(1-rPct) < pctTaper) { c.rDrive *= Math.abs((1-rPct)/pctTaper); - if ( Math.abs(robot.rRate.pidGet()) < 0.05) { - rMult += 0.05; + if (Math.abs(robot.rRate.pidGet()) < 0.5) { + rMin = c.rDrive = Math.max(c.rDrive, rMin) * 1.05; } - c.rDrive *= rMult; if (c.rDrive > rSpeed) { c.rDrive = rSpeed; } @@ -103,7 +101,12 @@ public class Autonomous { c.rDrive *= pctThrottle; } } - + SmartDashboard.putNumber("lPct", lPct); + SmartDashboard.putNumber("rPct", rPct); + SmartDashboard.putNumber("lMin", lMin); + SmartDashboard.putNumber("rMin", rMin); + SmartDashboard.putNumber("lDrive", c.lDrive); + SmartDashboard.putNumber("rDrive", c.rDrive); return lPct >= 1 && rPct >= 1; } } @@ -133,15 +136,15 @@ public class Autonomous { // Center peg /* commands = new Command[]{ - driveDistance(robot, 0.3, 8.75), + driveDistance(robot, 0.4, 8.75), }; */ // Left peg commands = new Command[]{ - driveDistance(robot, 0.3, 3.5), - turnDegrees(robot, 0.3, 60), - driveDistance(robot, 0.3, 8.3), + driveDistance(robot, 0.4, 3.5), + turnDegrees(robot, 0.4, 60), + driveDistance(robot, 0.4, 8.3), }; } |