summaryrefslogtreecommitdiff
path: root/src/org/usfirst/frc/team4272/robot2016/Teleop.java
diff options
context:
space:
mode:
Diffstat (limited to 'src/org/usfirst/frc/team4272/robot2016/Teleop.java')
-rw-r--r--src/org/usfirst/frc/team4272/robot2016/Teleop.java223
1 files changed, 129 insertions, 94 deletions
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
generated by cgit v1.2.3 (git 2.25.1) at 2024-04-28 06:48:09 +0000