diff options
Diffstat (limited to 'src/org/usfirst/frc/team4272/robot2016/Autonomus.java')
| -rw-r--r-- | src/org/usfirst/frc/team4272/robot2016/Autonomus.java | 243 |
1 files changed, 243 insertions, 0 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2016/Autonomus.java b/src/org/usfirst/frc/team4272/robot2016/Autonomus.java new file mode 100644 index 0000000..f3c4483 --- /dev/null +++ b/src/org/usfirst/frc/team4272/robot2016/Autonomus.java @@ -0,0 +1,243 @@ +/**
+* Copyright (c) 2017 Luke Shumaker.
+* Copyright (c) 2017 Thomas Griffith.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above copyright
+* notice, this list of conditions and the following disclaimer in the
+* documentation and/or other materials provided with the distribution.
+* * Neither the name of the FIRST nor the
+* names of its contributors may be used to endorse or promote products
+* derived from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY FIRST AND CONTRIBUTORS``AS IS'' AND ANY
+* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+* WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
+* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+package org.usfirst.frc.team4272.robot2017;
+
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class Autonomous {
+ private static interface Command {
+ /** Mutate the @{code Control} (possible because it is
+ * passed by reference), and return whether or not
+ * this command has finished yet.
+ *
+ * @param c The structure to control the robot
+ * @return Whether the command has finished.
+ */
+ public boolean execute(Control c);
+ }
+
+ private static class DriveDistance implements Command {
+ private static final double pctTaper = 1.0/6;
+ private static final double pctTolerance = 0.002;
+
+ private final HwRobot robot;
+ private final double pctThrottle;
+ private final double lSpeed, lDistTarget;
+ private final double rSpeed, rDistTarget;
+ private double lMin = 0, rMin = 0;
+ 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;
+ }
+ private boolean initialized = false;
+ public boolean execute(Control c) {
+ if (!initialized) {
+ robot.lDriveE.reset();
+ robot.rDriveE.reset();
+ initialized = true;
+ }
+ double lDistCurr = robot.lDriveE.getDistance();
+ double rDistCurr = robot.rDriveE.getDistance();
+ double lPct = lDistCurr/lDistTarget;
+ double rPct = rDistCurr/rDistTarget;
+
+ /* Left side */
+ c.lDrive = lSpeed;
+ if (Math.abs(1-lPct) < pctTaper) {
+ /* Taper off, slowing down as we approach the designed target */
+ c.lDrive *= Math.abs((1-lPct)/pctTaper);
+ if (Math.abs(robot.lRate.pidGet()) < 0.5) {
+ /* oops, tapered off too quickly */
+ lMin = c.lDrive = Math.max(c.lDrive, lMin) * 1.05; /* Multiplier */
+ }
+ if (c.lDrive > lSpeed) { /* Speed too high */
+ c.lDrive = lSpeed;
+ }
+ }
+ c.lDrive *= Math.signum(lDistTarget) * Math.signum(1-lPct);
+
+ /* Right side */
+ c.rDrive = rSpeed;
+ if (Math.abs(1-rPct) < pctTaper) {
+ /* Taper off, slowing down as we approach the designated target */
+ c.rDrive *= Math.abs((1-rPct)/pctTaper);
+ if (Math.abs(robot.rRate.pidGet()) < 0.5) {
+ /* oops, tapered off too quickly */
+ rMin = c.rDrive = Math.max(c.rDrive, rMin) * 1.05; /* Multiplier */
+ }
+ if (c.rDrive > rSpeed) { /* Speed too high */
+ c.rDrive = rSpeed;
+ }
+ }
+ c.rDrive *= Math.signum(rDistTarget) * Math.signum(1-rPct);
+
+ /* Make sure the left and right sides are even */
+ if (Math.abs(lPct - rPct) > pctTolerance) {
+ if (lPct > rPct) {
+ c.lDrive *= pctThrottle;
+ } else {
+ 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;
+ }
+ }
+ private static class Stop implements Command {
+ public boolean execute(Control c) {
+ c.lDrive = c.rDrive = 0;
+ return true;
+ }
+ }
+ private static class Init implements Command {
+ public boolean execute(Control c) {
+ c.lDrive = c.rDrive = 0;
+ c.highGear = c.gedOut = false;
+ return true;
+ }
+ }
+ private static class TimedCommand implements Command {
+ private final Command inner;
+ private final double secs;
+ private final Timer t = new Timer();
+ private boolean initialized = false;
+ public TimedCommand(Command inner, double secs) {
+ this.inner = inner;
+ this.secs = secs;
+ }
+ public boolean execute(Control c) {
+ if (!initialized) {
+ t.reset();
+ t.start();
+ initialized = true;
+ }
+ if (t.get() < secs) {
+ return inner.execute(c);
+ } else {
+ return true;
+ }
+ }
+ }
+
+ private static final double axleWidth = 2; // in feet
+ private static Command driveDistance(HwRobot robot, double speed, double 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, 0.6,
+ speed, Math.copySign(rad*axleWidth/2, rad),
+ speed, Math.copySign(rad*axleWidth/2, -rad));
+ }
+ 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;
+ private int step = 0;
+ private static final SendableChooser modeChooser = new SendableChooser();
+
+ public static void robotInit() {
+ modeChooser.addObject("Center Peg", "Center Peg");
+ modeChooser.addObject("Left Peg", "Left Peg");
+ modeChooser.addObject("Right Peg", "Right Peg");
+ modeChooser.addObject("Stand Still", "Stand Still");
+ SmartDashboard.putData("Autonomous Mode", modeChooser);
+ }
+
+ public Autonomous(HwRobot robot) {
+ this.robot = robot;
+
+ String mode = (String) modeChooser.getSelected();
+ SmartDashboard.putString("read mode", mode);
+
+ switch (mode) {
+ case "Center Peg":
+ commands = new Command[]{
+ new Init(),
+ new TimedCommand(driveDistance(robot, 0.4, 8.75), 6),
+ stop(robot),
+ new Command() {public boolean execute(Control c) {
+ c.gedOut = true;
+ return true;
+ }},
+ driveDistance(robot, 0.4, -3),
+ stop(robot),
+ };
+ break;
+ case "Left Peg":
+ commands = new Command[]{
+ new Init(),
+ driveDistance(robot, 0.4, 3.5),
+ turnDegrees(robot, 0.4, 60),
+ driveDistance(robot, 0.4, 8.3),
+ stop(robot),
+ };
+ break;
+ case "Right Peg":
+ commands = new Command[]{
+ new Init(),
+ driveDistance(robot, 0.4, 3.5),
+ turnDegrees(robot, 0.4, -60),
+ driveDistance(robot, 0.4, 8.3),
+ stop(robot),
+ };
+ break;
+ default:
+ commands = new Command[]{stop(robot)};
+ }
+ }
+
+ public Control run(Control c) {
+ if (step < commands.length) {
+ if (commands[step].execute(c))
+ step++;
+ }
+ SmartDashboard.putNumber("step", step);
+ return c;
+ }
+}
\ No newline at end of file |