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