1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
|
/**
* 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;
}
}
|
