/** * 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; } }