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