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 --- src/org/usfirst/frc/team4272/robot2016/Teleop.java | 223 ++++++++++++--------- 1 file changed, 129 insertions(+), 94 deletions(-) (limited to 'src/org/usfirst/frc/team4272/robot2016/Teleop.java') diff --git a/src/org/usfirst/frc/team4272/robot2016/Teleop.java b/src/org/usfirst/frc/team4272/robot2016/Teleop.java index 73b7b60..e0b5119 100644 --- a/src/org/usfirst/frc/team4272/robot2016/Teleop.java +++ b/src/org/usfirst/frc/team4272/robot2016/Teleop.java @@ -1,94 +1,129 @@ -package org.usfirst.frc.team4272.robot2016; - -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.Timer; -import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Axis; -import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Button; -import org.usfirst.frc.team4272.robotlib.PushButton; - -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(); - - public Teleop(HwRobot robot) { - this.robot = robot; - robot.armE.reset(); - } - - 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.lDrive1 = jsScale(oi.lStick); - control.rDrive1 = jsScale(oi.rStick); - control.lDrive2 = oi.lStick.getTrigger() ? control.lDrive1 : 0; - control.rDrive2 = oi.rStick.getTrigger() ? control.rDrive1 : 0; - control.arm = -oi.xbox.getAxis(Axis.LY); - if (control.arm < 0) - control.arm = control.arm*0.2; - - double fBall = oi.xbox.getAxis(Axis.RTrigger) - oi.xbox.getAxis(Axis.LTrigger); - double bBall = bound(-1, fBall, 0) + oneIf(oi.xbox.getButton(Button.A)) - oneIf(oi.xbox.getButton(Button.B)); - - boolean ballL = robot.ballL.get(); - control.fBall = bound(-oneIf(ballL), fBall < 0 ? 0.5*fBall : fBall, 1); - control.bBall = bound(-oneIf(ballL), bBall < 0 ? 0.5*bBall : bBall, 1); - - /* I'm eyeballing 10 degrees ≈ 50 clicks */ - if (robot.armE.pidGet() < 50 && !robot.armL.get()) { - robot.armE.reset(); - } - - if (oi.xbox.getButton(Button.LBumper)) - control.arm = 1-(robot.armE.pidGet()/armSoftStop); - else if (oi.xbox.getButton(Button.RBumper)) - control.arm = robot.armE.pidGet()/armSoftStop; - - if (shootButton.update(oi.xbox.getButton(Button.X))) { - time.reset(); - time.start(); - } - if (oi.xbox.getButton(Button.X)) { - if (time.get() < 0.5) { - control.fBall = control.bBall = -0.5; - } else if (time.get() < 2.5) { - control.bBall = -0.5; - control.fBall = 1; - } else { - control.bBall = 1; - control.fBall = 1; - } - } - - //if (robot.armE.pidGet() > armSoftStop && control.arm > 0) - // control.arm = 0; - - /* Take pictures */ - /* - if (camButton.update(oi.xbox.getButton(Button.RT))) { - try { - ProcessBuilder pb = new ProcessBuilder("/home/lvuser/tweeterbot/bin/snapPhoto"); - pb.redirectErrorStream(true); - pb.start(); - } catch (Exception e) {}; - } - */ - - return control; - } -} +/** +* 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.Timer; +import edu.wpi.first.wpilibj.Preferences; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import org.usfirst.frc.team4272.robotlib.PushButton; + +public class Teleop { + private final HwRobot robot; + private final Timer timer = new Timer(); + private final Preferences prefs = Preferences.getInstance(); + + private double lastTime; + private double currentTime; + private boolean state = false; + + public Teleop(HwRobot robot) { + this.robot = robot; + timer.start(); + } + + 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.rStick.getTrigger()) { + control.highGear = true; + } else if (oi.lStick.getTrigger()) { + control.highGear = false; + } else { + double speed = Math.abs((robot.lRate.pidGet() + robot.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)) { + if (timer.get() > 0.30) { + timer.reset(); + } + if (timer.get() < 0.15) { + control.lDrive = 0.5; + control.rDrive = 0; + } else { + control.lDrive = 0; + control.rDrive = 0.5; + } + } + + /* climber */ + control.climber = oi.xbox.getY(Hand.kLeft); + if (control.climber <= 0.2) { + 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; + } +} \ No newline at end of file -- cgit v1.2.3