From 2d0eec1f93f1ee2ff7cd01bcafc737914146c561 Mon Sep 17 00:00:00 2001 From: Luke Shumaker Date: Tue, 4 Apr 2017 17:31:40 -0400 Subject: side peg auto --- .../usfirst/frc/team4272/robot2017/Autonomous.java | 55 ++++++++++++++-------- 1 file 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); -- cgit v1.2.3-54-g00ecf