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
|
package org.usfirst.frc.team4272.robot2016;
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;
}
/* gear place */
if (oi.xbox.getAButton()) {
control.gedOut = true;
}
if (oi.xbox.getBButton()) {
control.gedOut = false;
}
/* 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;
}
}
|