blob: 95a47b795b85c0f715dc1531b7fa4e4c8f2e0711 (
plain)
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
|
package org.usfirst.frc.team4272.robot2015;
import org.usfirst.frc.team4272.robotlib.Toggler;
import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Axis;
import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Button;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Relay;
/**
* The VM is configured to automatically run this class, and to call the
* functions corresponding to each mode, as described in the IterativeRobot
* documentation. If you change the name of this class or the package after
* creating this project, you must also update the manifest file in the resource
* directory.
*/
public class Robot extends IterativeRobot {
private HwRobot robot = new HwRobot();
private HwOI oi;
/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
*/
public void robotInit() {
}
public void autonomousInit() {
}
public void autonomousPeriodic() {
}
private Toggler grabButton = new Toggler();
private Toggler pushButton = new Toggler();
private Toggler ledsButton = new Toggler();
public void teleopInit() {
//robot.lDrive.reset();
//robot.rDrive.reset();
//robot.winch.reset();
//robot.lDrive.enable();
//robot.rDrive.enable();
//robot.winch.enable();
robot.grab.setEnabled(true);
robot.push.setEnabled(true);
oi = new HwOI();
grabButton.update(false);
pushButton.update(false);
ledsButton.update(false);
}
public void teleopPeriodic() {
/* Drive */
robot.lDriveM.pidWrite(-oi.lStick.getY()*Math.abs(oi.lStick.getY()));
robot.rDriveM.pidWrite( oi.rStick.getY()*Math.abs(oi.rStick.getY()));
/* Winch */
double w = oi.xbox.getAxis(Axis.LY);
if (Math.abs(w) < 0.1) { w = 0; }
robot.winchM.pidWrite(-w);
/* left intake */
if (oi.xbox.getButton(Button.LB)) {
robot.lIntake.set(Relay.Value.kReverse);
} else if (oi.xbox.getAxis(Axis.LT) > 0.75) {
robot.lIntake.set(Relay.Value.kForward);
} else {
robot.lIntake.set(Relay.Value.kOff);
}
/* right intake */
if (oi.xbox.getButton(Button.RB)) {
robot.rIntake.set(Relay.Value.kReverse);
} else if (oi.xbox.getAxis(Axis.RT) > 0.75) {
robot.rIntake.set(Relay.Value.kForward);
} else {
robot.rIntake.set(Relay.Value.kOff);
}
/* grab */
robot.grab.setForward(grabButton.update(oi.xbox.getButton(Button.A)));
robot.push.setForward(pushButton.update(oi.xbox.getButton(Button.B)));
if (ledsButton.update(oi.xbox.getButton(Button.RT))) {
robot.lights.set(Relay.Value.kForward);
} else {
robot.lights.set(Relay.Value.kOff);
}
}
public void disabledInit() {
//robot.lDrive.reset();
//robot.rDrive.reset();
//robot.winch.reset();
robot.grab.setEnabled(false);
robot.push.setEnabled(false);
}
/**
* This function is called periodically during test mode
*/
public void testPeriodic() {
}
}
|
