diff options
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/Autonomous.java | 26 |
1 files changed, 17 insertions, 9 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java index 8398688..7220d89 100644 --- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java +++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java @@ -71,31 +71,38 @@ public class Autonomous { double rDistCurr = robot.rDriveE.getDistance(); double lPct = lDistCurr/lDistTarget; double rPct = rDistCurr/rDistTarget; - /* Left Drive train */ + + /* Left side */ c.lDrive = lSpeed; if (Math.abs(1-lPct) < pctTaper) { - c.lDrive *= Math.abs((1-lPct)/pctTaper); /* Tapering off to slow down to the designed target*/ + /* Taper off, slowing down as we approach the designed target */ + c.lDrive *= Math.abs((1-lPct)/pctTaper); if (Math.abs(robot.lRate.pidGet()) < 0.5) { - lMin = c.lDrive = Math.max(c.lDrive, lMin) * 1.05; /* Multiplier*/ + /* oops, tapered off too quickly */ + lMin = c.lDrive = Math.max(c.lDrive, lMin) * 1.05; /* Multiplier */ } - if (c.lDrive > lSpeed) { /* Speed to high*/ + if (c.lDrive > lSpeed) { /* Speed too high */ c.lDrive = lSpeed; } } c.lDrive *= Math.signum(lDistTarget) * Math.signum(1-lPct); - /* Right Drive Train*/ + + /* Right side */ c.rDrive = rSpeed; if (Math.abs(1-rPct) < pctTaper) { - c.rDrive *= Math.abs((1-rPct)/pctTaper); /* Tapering off to slow down to the deigned target*/ + /* Taper off, slowing down as we approach the designated target */ + c.rDrive *= Math.abs((1-rPct)/pctTaper); if (Math.abs(robot.rRate.pidGet()) < 0.5) { - rMin = c.rDrive = Math.max(c.rDrive, rMin) * 1.05; /* Multiplier*/ + /* oops, tapered off too quickly */ + rMin = c.rDrive = Math.max(c.rDrive, rMin) * 1.05; /* Multiplier */ } - if (c.rDrive > rSpeed) { /* Speed to high*/ + if (c.rDrive > rSpeed) { /* Speed too high */ c.rDrive = rSpeed; } } c.rDrive *= Math.signum(rDistTarget) * Math.signum(1-rPct); - /* If lDrive or rDrive is not even */ + + /* Make sure the left and right sides are even */ if (Math.abs(lPct - rPct) > pctTolerance) { if (lPct > rPct) { c.lDrive *= pctThrottle; @@ -103,6 +110,7 @@ public class Autonomous { c.rDrive *= pctThrottle; } } + SmartDashboard.putNumber("lPct", lPct); SmartDashboard.putNumber("rPct", rPct); SmartDashboard.putNumber("lMin", lMin); |