Commit | Line | Data |
---|---|---|
b449387d LH |
1 | |
2 | package org.usfirst.frc.team3501.robot; | |
3 | ||
4 | import edu.wpi.first.wpilibj.IterativeRobot; | |
5 | import edu.wpi.first.wpilibj.command.Command; | |
6 | import edu.wpi.first.wpilibj.command.Scheduler; | |
7 | import edu.wpi.first.wpilibj.livewindow.LiveWindow; | |
3e4790a8 LH |
8 | import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; |
9 | import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | |
b449387d | 10 | |
3e4790a8 | 11 | import org.usfirst.frc.team3501.robot.autons.*; |
b449387d LH |
12 | import org.usfirst.frc.team3501.robot.subsystems.*; |
13 | ||
14 | public class Robot extends IterativeRobot { | |
15 | ||
f24c549d LH |
16 | public static Drivetrain drivetrain; |
17 | public static Arm arm; | |
18 | public static Claw claw; | |
19 | ||
20 | public static Pneumatics pneumatics; | |
b449387d LH |
21 | |
22 | public static OI oi; | |
23 | ||
3e4790a8 LH |
24 | private SendableChooser autoChooser; |
25 | ||
26 | private Command autonomousCommand; | |
b449387d LH |
27 | |
28 | public void robotInit() { | |
29 | oi = new OI(); | |
30 | ||
f24c549d LH |
31 | drivetrain = new Drivetrain(); |
32 | arm = new Arm(); | |
33 | claw = new Claw(); | |
34 | ||
35 | pneumatics = new Pneumatics(); | |
36 | ||
3e4790a8 | 37 | chooseAuto(); |
b449387d LH |
38 | } |
39 | ||
40 | public void disabledPeriodic() { | |
41 | Scheduler.getInstance().run(); | |
42 | } | |
43 | ||
44 | public void autonomousInit() { | |
f24c549d LH |
45 | pneumatics.start(); |
46 | ||
3e4790a8 | 47 | autonomousCommand = (Command) autoChooser.getSelected(); |
b449387d LH |
48 | autonomousCommand.start(); |
49 | } | |
50 | ||
51 | public void autonomousPeriodic() { | |
52 | Scheduler.getInstance().run(); | |
53 | } | |
54 | ||
55 | public void teleopInit() { | |
f24c549d LH |
56 | pneumatics.start(); |
57 | ||
b449387d LH |
58 | autonomousCommand.cancel(); |
59 | } | |
60 | ||
61 | public void teleopPeriodic() { | |
62 | Scheduler.getInstance().run(); | |
b449387d LH |
63 | } |
64 | ||
65 | public void testPeriodic() { | |
66 | LiveWindow.run(); | |
67 | } | |
3e4790a8 | 68 | |
f24c549d LH |
69 | public void disabledInit() { |
70 | pneumatics.stop(); | |
71 | } | |
72 | ||
3e4790a8 LH |
73 | private void chooseAuto() { |
74 | autoChooser = new SendableChooser(); | |
75 | ||
76 | autoChooser.addDefault("Drive over step", new DriveOverStep()); | |
77 | autoChooser.addObject("Drive past step", new DrivePastStep()); | |
78 | ||
79 | SmartDashboard.putData("Auto Mode", autoChooser); | |
80 | } | |
b449387d | 81 | } |