/** * 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.smartdashboard.SmartDashboard; public class Autonomous { private static interface Command { public boolean execute(Control c); } 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 lSpeed, lDistTarget; private final double rSpeed, rDistTarget; public DriveDistance(HwRobot robot, double lSpeed, double lDistTarget, double rSpeed, double rDistTarget) { this.robot = robot; 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) { 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; c.lDrive = lSpeed * Math.signum(lDistTarget) * Math.signum(1-lPct); if (Math.abs(1-lPct) < pctTaper) { c.lDrive *= Math.sqrt(Math.abs((1-lPct)/pctTaper)); } c.rDrive = rSpeed * Math.signum(rDistTarget) * Math.signum(1-rPct); if (Math.abs(1-rPct) < pctTaper) { c.rDrive *= Math.sqrt(Math.abs((1-rPct)/pctTaper)); } if (Math.abs(lPct - rPct) > pctTolerance) { if (lPct > rPct) { c.lDrive *= 0.6; //c.rDrive *= 2; } else { //c.lDrive *= 2; c.rDrive *= 0.6; } } 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); } private static Command turnRadians(HwRobot robot, double speed, double rad) { return new DriveDistance(robot, 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 final HwRobot robot; private final Command[] commands; private int step = 0; public Autonomous(HwRobot robot) { this.robot = robot; // Center peg /* commands = new Command[]{ driveDistance(robot, 0.3, 8.75), }; */ // 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), }; } public Control run(Control c) { c.gedOut = false; c.highGear = false; if (step < commands.length) { if (commands[step].execute(c)) step++; } SmartDashboard.putNumber("step", step); return c; } }