2 package org
.usfirst
.frc
.team3501
.robot
;
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
;
8 import edu
.wpi
.first
.wpilibj
.smartdashboard
.SendableChooser
;
9 import edu
.wpi
.first
.wpilibj
.smartdashboard
.SmartDashboard
;
11 import org
.usfirst
.frc
.team3501
.robot
.autons
.*;
12 import org
.usfirst
.frc
.team3501
.robot
.commands
.*;
13 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.*;
14 import org
.usfirst
.frc
.team3501
.util
.AutonData
;
16 public class Robot
extends IterativeRobot
{
18 public static Drivetrain drivetrain
;
19 public static Arm arm
;
20 public static Claw claw
;
22 public static Pneumatics pneumatics
;
26 public static AutonData autonData
;
28 private SendableChooser autonChooser
;
29 private Command autonomousCommand
;
31 public void robotInit() {
34 drivetrain
= new Drivetrain();
38 pneumatics
= new Pneumatics();
40 autonData
= new AutonData();
45 public void disabledPeriodic() {
46 Scheduler
.getInstance().run();
49 public void autonomousInit() {
50 schedule(new TurnOnCompressor());
54 autonomousCommand
= (Command
) autonChooser
.getSelected();
55 autonomousCommand
.start();
58 public void autonomousPeriodic() {
59 Scheduler
.getInstance().run();
62 public void teleopInit() {
63 schedule(new TurnOnCompressor());
65 autonomousCommand
.cancel();
68 public void teleopPeriodic() {
69 Scheduler
.getInstance().run();
72 public void testPeriodic() {
76 private void chooseAuto() {
77 autonChooser
= new SendableChooser();
79 autonChooser
.addDefault("Pick up container", new ContainerOverStep());
80 autonChooser
.addObject("Drive over step", new DriveOverStep());
81 autonChooser
.addObject("Drive past step", new DrivePastStep());
83 SmartDashboard
.putData("Auto Mode", autonChooser
);
86 private void schedule(Command c
) {
87 Scheduler
.getInstance().add(c
);