summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/org/usfirst/frc/team4272/robot2017/Autonomous.java23
1 files changed, 18 insertions, 5 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
index 5fa7257..5a62be4 100644
--- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
+++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
@@ -72,7 +72,7 @@ public class Autonomous {
double lPct = lDistCurr/lDistTarget;
double rPct = rDistCurr/rDistTarget;
- c.lDrive = lSpeed * Math.signum(lDistTarget) * Math.signum(1-lPct);
+ c.lDrive = lSpeed;
if (Math.abs(1-lPct) < pctTaper) {
c.lDrive *= Math.abs((1-lPct)/pctTaper);
if (Math.abs(robot.lRate.pidGet()) < 0.5) {
@@ -82,8 +82,9 @@ public class Autonomous {
c.lDrive = lSpeed;
}
}
+ c.lDrive *= Math.signum(lDistTarget) * Math.signum(1-lPct);
- c.rDrive = rSpeed * Math.signum(rDistTarget) * Math.signum(1-rPct);
+ c.rDrive = rSpeed;
if (Math.abs(1-rPct) < pctTaper) {
c.rDrive *= Math.abs((1-rPct)/pctTaper);
if (Math.abs(robot.rRate.pidGet()) < 0.5) {
@@ -93,6 +94,7 @@ public class Autonomous {
c.rDrive = rSpeed;
}
}
+ c.rDrive *= Math.signum(rDistTarget) * Math.signum(1-rPct);
if (Math.abs(lPct - rPct) > pctTolerance) {
if (lPct > rPct) {
@@ -110,6 +112,12 @@ public class Autonomous {
return lPct >= 1 && rPct >= 1;
}
}
+ private static class Stop implements Command {
+ public boolean execute(Control c) {
+ c.lDrive = c.rDrive = 0;
+ return true;
+ }
+ }
private static final double axleWidth = 2; // in feet
private static Command driveDistance(HwRobot robot, double speed, double dist) {
@@ -125,6 +133,9 @@ public class Autonomous {
private static Command turnDegrees(HwRobot robot, double speed, double deg) {
return turnRadians(robot, speed, deg * Math.PI/180);
}
+ private static Command stop(HwRobot robot) {
+ return new Stop();
+ }
private final HwRobot robot;
private final Command[] commands;
@@ -133,19 +144,21 @@ public class Autonomous {
public Autonomous(HwRobot robot) {
this.robot = robot;
- // Center peg
- /*
+ // Cross line / center peg
commands = new Command[]{
driveDistance(robot, 0.4, 8.75),
+ stop(robot),
};
- */
// Left peg
+ /*
commands = new Command[]{
driveDistance(robot, 0.4, 3.5),
turnDegrees(robot, 0.4, 60),
driveDistance(robot, 0.4, 8.3),
+ stop(robot),
};
+ */
}
public Control run(Control c) {