challenge-bot.com Git - 3501/3501-spark-go/blob - src/org/usfirst/frc/team3501/robot/Robot.java

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