From 4e2b11e4df2f724a0e2d66c28bc465126a1c9d37 Mon Sep 17 00:00:00 2001 From: Deveveloper Date: Thu, 23 Mar 2017 01:47:29 -0400 Subject: Changes sitting on netbook --- .../usfirst/frc/team4272/robot2016/Autonomus.java | 243 +++++++++++++++++++++ 1 file changed, 243 insertions(+) create mode 100644 src/org/usfirst/frc/team4272/robot2016/Autonomus.java (limited to 'src/org/usfirst/frc/team4272/robot2016/Autonomus.java') 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 -- cgit v1.2.3