diff options
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/Autonomous.java | 31 | ||||
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2017/HwRobot.java | 3 |
2 files changed, 12 insertions, 22 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java index 038dd5a..da7b8f8 100644 --- a/src/org/usfirst/frc/team4272/robot2017/Autonomous.java +++ b/src/org/usfirst/frc/team4272/robot2017/Autonomous.java @@ -37,24 +37,22 @@ public class Autonomous { private static class DriveDistance implements Command { private static final double pctTaper = 1.0/3; private static final double pctTolerance = 0.005; - private static final double pctThrottle = 0.2; private static final double distFinished = 0.1; private final HwRobot robot; + private final double pctThrottle; private final double lSpeed, lDistTarget; private final double rSpeed, rDistTarget; - public DriveDistance(HwRobot robot, + public DriveDistance(HwRobot robot, double pctThrottle, double lSpeed, double lDistTarget, double rSpeed, double rDistTarget) { this.robot = robot; + this.pctThrottle = pctThrottle; this.lSpeed = lSpeed; this.lDistTarget = lDistTarget; this.rSpeed = rSpeed; this.rDistTarget = rDistTarget; } - public DriveDistance(HwRobot robot, double speed, double target) { - this(robot, speed, target, speed, target); - } private boolean initialized = false; public boolean execute(Control c) { if (!initialized) { @@ -79,27 +77,23 @@ public class Autonomous { if (Math.abs(lPct - rPct) > pctTolerance) { if (lPct > rPct) { - c.lDrive *= 0.6; - //c.rDrive *= 2; + c.lDrive *= pctThrottle; } else { - //c.lDrive *= 2; - c.rDrive *= 0.6; + c.rDrive *= pctThrottle; } } - SmartDashboard.putNumber("lPct", lPct); - SmartDashboard.putNumber("rPct", rPct); - SmartDashboard.putNumber("lTarget", lDistTarget); - SmartDashboard.putNumber("rTarget", rDistTarget); return Math.abs(lDistCurr - lDistTarget) < distFinished && Math.abs(rDistCurr - rDistTarget) < distFinished; } } private static final double axleWidth = 2; // in feet private static Command driveDistance(HwRobot robot, double speed, double dist) { - return new DriveDistance(robot, speed, dist); + return new DriveDistance(robot, 0.2, + speed, dist, + speed, dist); } private static Command turnRadians(HwRobot robot, double speed, double rad) { - return new DriveDistance(robot, + return new DriveDistance(robot, 0.6, speed, Math.copySign(rad*axleWidth/2, rad), speed, Math.copySign(rad*axleWidth/2, -rad)); } @@ -123,10 +117,9 @@ public class Autonomous { // Left peg commands = new Command[]{ - //driveDistance(robot, 0.3, 3.5), - //turnDegrees(robot, 0.3, 60), - //driveDistance(robot, 0.3, 8.3), - turnDegrees(robot, 0.5, 90), + driveDistance(robot, 0.3, 3.5), + turnDegrees(robot, 0.3, 60), + driveDistance(robot, 0.3, 8.3), }; } diff --git a/src/org/usfirst/frc/team4272/robot2017/HwRobot.java b/src/org/usfirst/frc/team4272/robot2017/HwRobot.java index 7608376..091b68b 100644 --- a/src/org/usfirst/frc/team4272/robot2017/HwRobot.java +++ b/src/org/usfirst/frc/team4272/robot2017/HwRobot.java @@ -119,9 +119,6 @@ public class HwRobot { SmartDashboard.putBoolean("highGear", c.highGear); SmartDashboard.putBoolean("gedOut", c.gedOut); - SmartDashboard.putNumber("lDrive", c.lDrive); - SmartDashboard.putNumber("rDrive", c.rDrive); - //PDP.updateTable(); //SmartDashboard.putData("PDP", PDP); } |