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