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.*; |
c0ffc2de | 12 | import org.usfirst.frc.team3501.robot.commands.*; |
b449387d | 13 | import org.usfirst.frc.team3501.robot.subsystems.*; |
f139e313 | 14 | import org.usfirst.frc.team3501.util.AutonData; |
b449387d LH |
15 | |
16 | public class Robot extends IterativeRobot { | |
17 | ||
f24c549d LH |
18 | public static Drivetrain drivetrain; |
19 | public static Arm arm; | |
20 | public static Claw claw; | |
21 | ||
22 | public static Pneumatics pneumatics; | |
b449387d LH |
23 | |
24 | public static OI oi; | |
25 | ||
b2640783 | 26 | public static AutonData autonData; |
3e4790a8 | 27 | |
b2640783 | 28 | private SendableChooser autonChooser; |
3e4790a8 | 29 | private Command autonomousCommand; |
b449387d LH |
30 | |
31 | public void robotInit() { | |
32 | oi = new OI(); | |
33 | ||
f24c549d LH |
34 | drivetrain = new Drivetrain(); |
35 | arm = new Arm(); | |
36 | claw = new Claw(); | |
37 | ||
38 | pneumatics = new Pneumatics(); | |
39 | ||
b2640783 LH |
40 | autonData = new AutonData(); |
41 | ||
3e4790a8 | 42 | chooseAuto(); |
b449387d LH |
43 | } |
44 | ||
45 | public void disabledPeriodic() { | |
46 | Scheduler.getInstance().run(); | |
47 | } | |
48 | ||
49 | public void autonomousInit() { | |
c0ffc2de | 50 | schedule(new TurnOnCompressor()); |
f24c549d | 51 | |
b2640783 LH |
52 | autonData.update(); |
53 | ||
54 | autonomousCommand = (Command) autonChooser.getSelected(); | |
b449387d LH |
55 | autonomousCommand.start(); |
56 | } | |
57 | ||
58 | public void autonomousPeriodic() { | |
59 | Scheduler.getInstance().run(); | |
60 | } | |
61 | ||
62 | public void teleopInit() { | |
c0ffc2de | 63 | schedule(new TurnOnCompressor()); |
f24c549d | 64 | |
b449387d LH |
65 | autonomousCommand.cancel(); |
66 | } | |
67 | ||
68 | public void teleopPeriodic() { | |
69 | Scheduler.getInstance().run(); | |
b449387d LH |
70 | } |
71 | ||
72 | public void testPeriodic() { | |
73 | LiveWindow.run(); | |
74 | } | |
3e4790a8 LH |
75 | |
76 | private void chooseAuto() { | |
b2640783 | 77 | autonChooser = new SendableChooser(); |
3e4790a8 | 78 | |
b2640783 LH |
79 | autonChooser.addDefault("Pick up container", new ContainerOverStep()); |
80 | autonChooser.addObject("Drive over step", new DriveOverStep()); | |
81 | autonChooser.addObject("Drive past step", new DrivePastStep()); | |
3e4790a8 | 82 | |
b2640783 | 83 | SmartDashboard.putData("Auto Mode", autonChooser); |
3e4790a8 | 84 | } |
c0ffc2de LH |
85 | |
86 | private void schedule(Command c) { | |
87 | Scheduler.getInstance().add(c); | |
88 | } | |
b449387d | 89 | } |