summaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
authorLuke Shumaker <lukeshu@lukeshu.com>2017-04-04 17:31:40 -0400
committerLuke Shumaker <lukeshu@lukeshu.com>2017-04-04 17:31:40 -0400
commit2d0eec1f93f1ee2ff7cd01bcafc737914146c561 (patch)
tree55a5818ce6cfa61edde4f20494552c79683b4213 /src
parent31a99043d50cae624c956a8a8f80146a1c430d16 (diff)
side peg auto
Diffstat (limited to 'src')
-rw-r--r--src/org/usfirst/frc/team4272/robot2017/Autonomous.java55
1 files changed, 35 insertions, 20 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
index 60dcd24..8e5846a 100644
--- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
+++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java
@@ -95,16 +95,30 @@ public class Autonomous {
});
modeChooser.addObject("Left Peg", robot->new Command[]{
init,
- driveDistance(robot, 0.5, 8.0),
- turnDegrees(robot, 0.4, 60),
+ driveDistance(robot, 0.4, 8.4),
+ turnDegrees(robot, 0.4, 70),
driveDistance(robot, 0.4, 2.16),
+ timeout(1.0, c->false),
+ c->{c.gedOut = true; return true;},
+ timeout(1.0, c->false),
+ driveDistance(robot, 0.4, -3),
+ c->{c.gedOut = false; return true;},
+ turnDegrees(robot, 0.4, -70),
+ driveDistance(robot, 1.0, 15),
stop,
});
modeChooser.addObject("Right Peg", robot->new Command[]{
init,
- driveDistance(robot, 0.4, 3.5),
- turnDegrees(robot, 0.4, -60),
- driveDistance(robot, 0.4, 8.3),
+ driveDistance(robot, 0.4, 8.4),
+ turnDegrees(robot, 0.4, -70),
+ driveDistance(robot, 0.4, 2.16),
+ timeout(1.0, c->false),
+ c->{c.gedOut = true; return true;},
+ timeout(1.0, c->false),
+ driveDistance(robot, 0.4, -3),
+ c->{c.gedOut = false; return true;},
+ turnDegrees(robot, 0.4, 70),
+ driveDistance(robot, 1.0, 15),
stop,
});
modeChooser.addObject("Stand Still", robot->new Command[]{
@@ -229,16 +243,16 @@ public class Autonomous {
}
c.rDrive *= Math.signum(rDistTarget) * Math.signum(1-rPct);
- /* Make sure the left and right sides are even */
cntT++;
- if (Math.abs(lPct - rPct) > pctTolerance) {
- if (lPct > rPct) {
- cntL++;
- c.lDrive *= pctThrottle;
- } else {
- cntR++;
- c.rDrive *= pctThrottle;
- }
+ double throttle = Math.min(Math.abs(lPct - rPct), pctTolerance)/pctTolerance;
+ if (lPct > rPct) {
+ c.lDrive *= 1.0-(pctThrottle*throttle);
+ //c.rDrive /= 1.0-(pctThrottle*throttle);
+ cntL += throttle;
+ } else {
+ //c.lDrive /= 1.0-(pctThrottle*throttle);
+ c.rDrive *= 1.0-(pctThrottle*throttle);
+ cntR += throttle;
}
SmartDashboard.putNumber("lPct", lPct);
@@ -247,8 +261,8 @@ public class Autonomous {
SmartDashboard.putNumber("rMin", rMin);
SmartDashboard.putNumber("lDrive", c.lDrive);
SmartDashboard.putNumber("rDrive", c.rDrive);
- SmartDashboard.putNumber("lCnt", cntL/(double)cntT);
- SmartDashboard.putNumber("rCnt", cntR/(double)cntT);
+ SmartDashboard.putNumber("lCnt", cntL/cntT);
+ SmartDashboard.putNumber("rCnt", cntR/cntT);
return lPct >= 1 && rPct >= 1;
}
}
@@ -276,14 +290,15 @@ public class Autonomous {
}
private static Command driveDistance(HwRobot robot, double speed, double dist) {
- return new DriveDistance(robot, 0.05, 0.001,
+ return new DriveDistance(robot, 1.0, (1/36.0)/dist,
speed, dist,
speed, dist);
}
private static Command turnRadians(HwRobot robot, double speed, double rad) {
- return new DriveDistance(robot, 0.6, 0.002,
- speed, Math.copySign(rad*robot.axleWidth/2, rad),
- speed, Math.copySign(rad*robot.axleWidth/2, -rad));
+ double dist = rad*robot.axleWidth/2;
+ return new DriveDistance(robot, 0.33, (1/36.0)/dist,
+ speed, Math.copySign(dist, rad),
+ speed, Math.copySign(dist, -rad));
}
private static Command turnDegrees(HwRobot robot, double speed, double deg) {
return turnRadians(robot, speed, deg * Math.PI/180);