summaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/org/usfirst/frc/team4272/robot2017/Autonomous.java29
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),
};
}