/** * Copyright (c) 2015-2017 Luke Shumaker. * Copyright (c) 2017 Cameron Richards. * Copyright (c) 2017 Noah Gaeta. * 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.GenericHID.Hand; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.PIDSource; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.Preferences; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import org.usfirst.frc.team4272.robotlib.PushButton; import org.usfirst.frc.team4272.robotlib.RollingAvg; public class Teleop { private final HwRobot robot; private final double armSoftStop = 95;//295; private final PushButton shootButton = new PushButton(); private final Timer time = new Timer(); private double lastTime; private double currentTime; private boolean state = false; private PIDSource lRate, rRate; private Preferences prefs = Preferences.getInstance(); public Teleop(HwRobot robot) { this.robot = robot; this.lRate = new RollingAvg(5, robot.lDriveE); this.rRate = new RollingAvg(5, robot.rDriveE); } private static double jsScale(Joystick j) { double y = j.getY();/* +:forward; -:backward */ double z = j.getZ();/* +:more-sensitive; -:less-sensitive */ return Math.copySign(Math.pow(Math.abs(y), 2.0-z), y); } private static double bound(double min, double v, double max) { return Math.min(Math.max(v, min), max); } private static double oneIf(boolean b) { return b ? 1 : 0; } public Control run(Control control, HwOI oi) { /* drive */ control.lDrive = -jsScale(oi.lStick); control.rDrive = -jsScale(oi.rStick); /* shifting */ double shiftUp = prefs.getDouble("shiftUp", 3.3*3.28); double shiftDown = prefs.getDouble("shiftDown", 2.7*3.28); if (oi.lStick.getTrigger()) { control.highGear = false; } else if (oi.rStick.getTrigger()) { control.highGear = true; } else { double speed = Math.abs((lRate.pidGet() + rRate.pidGet()) / 2); if (!control.highGear) { if (speed > shiftUp) control.highGear = true; } else { if (speed < shiftDown) control.highGear = false; } } /* auto gear wiggle */ if (oi.rStick.getRawButton(2) || oi.lStick.getRawButton(2)) { currentTime = Timer.getMatchTime(); if (currentTime-lastTime > 0.15) { state = !state; lastTime = currentTime; } if (state) { control.lDrive = 0.5; control.rDrive = 0; } else { control.lDrive = 0; control.rDrive = 0.5; } } /* climber */ if (Math.abs(oi.xbox.getY(Hand.kLeft)) > 0.2) { control.climber = oi.xbox.getY(Hand.kLeft); } else { control.climber = 0; } /* GED */ control.gedOut = oi.xbox.getTriggerAxis(Hand.kRight) > 0.5; /* camera */ // AnnaLaura wants the GED on the trigger, so we'll // have to find somewhere else for the camera. /* if (oi.xbox.getTriggerAxis(Hand.kRight) > 0) { control.camRotate = 1; } else if (oi.xbox.getTriggerAxis(Hand.kLeft) > 0) { control.camRotate = -1; } else { control.camRotate = 0; } control.camTilt = oi.xbox.getX(Hand.kRight); */ return control; } }