Commit | Line | Data |
---|---|---|
78b997ee ME |
1 | package org.usfirst.frc.team3501.robot.commands.driving; |
2 | ||
a784e9d2 | 3 | import org.usfirst.frc.team3501.robot.OI; |
78b997ee ME |
4 | import org.usfirst.frc.team3501.robot.Robot; |
5 | ||
8275a069 | 6 | import edu.wpi.first.wpilibj.Joystick.AxisType; |
78b997ee ME |
7 | import edu.wpi.first.wpilibj.command.Command; |
8 | ||
9 | /** | |
3ef4d379 | 10 | * This command will run throughout teleop and listens for joystick inputs to |
f0a71840 CZ |
11 | * drive the driveTrain. This never finishes until teleop ends. - works in |
12 | * conjunction with OI.java | |
78b997ee ME |
13 | */ |
14 | public class JoystickDrive extends Command { | |
15 | ||
f74d236d CZ |
16 | double previousThrust = 0; |
17 | double previousTwist = 0; | |
18 | ||
d17d868d | 19 | public JoystickDrive() { |
20 | requires(Robot.getDriveTrain()); | |
21 | } | |
22 | ||
23 | @Override | |
24 | protected void initialize() { | |
25 | } | |
26 | ||
27 | @Override | |
28 | protected void execute() { | |
f74d236d CZ |
29 | double thrust = OI.xboxController.getY(); |
30 | double twist = OI.xboxController.getAxis(AxisType.kZ); | |
8275a069 | 31 | |
f74d236d CZ |
32 | thrust = (6 * previousThrust + thrust) / 7; |
33 | twist = (6 * previousTwist + twist) / 7; | |
8275a069 | 34 | |
f74d236d CZ |
35 | previousThrust = thrust; |
36 | previousTwist = twist; | |
37 | ||
38 | Robot.getDriveTrain().joystickDrive(-thrust, -twist); | |
d17d868d | 39 | } |
40 | ||
41 | @Override | |
42 | protected boolean isFinished() { | |
43 | return false; | |
44 | } | |
45 | ||
46 | @Override | |
47 | protected void end() { | |
48 | Robot.getDriveTrain().stop(); | |
49 | } | |
50 | ||
51 | @Override | |
52 | protected void interrupted() { | |
53 | } | |
78b997ee | 54 | } |