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

   1 package org.usfirst.frc.team3501.robot;
   2
   3 import org.usfirst.frc.team3501.robot.Constants.Defense;
   4 import org.usfirst.frc.team3501.robot.commands.auton.ChooseStrategy;
   5 import org.usfirst.frc.team3501.robot.commands.driving.SetLowGear;
   6 import org.usfirst.frc.team3501.robot.commands.intakearm.Photogate;
   7 import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
   8 import org.usfirst.frc.team3501.robot.subsystems.IntakeArm;
   9 import org.usfirst.frc.team3501.robot.subsystems.Shooter;
  10
  11 import edu.wpi.first.wpilibj.IterativeRobot;
  12 import edu.wpi.first.wpilibj.command.Scheduler;
  13 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
  14 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
  15
  16 public class Robot extends IterativeRobot {
  17   public static OI oi;
  18   public static DriveTrain driveTrain;
  19   public static Shooter shooter;
  20   public static IntakeArm intakeArm;
  21   public static Photogate photogate;
  22
  23   // Sendable Choosers send a drop down menu to the Smart Dashboard.
  24   private static SendableChooser defenseChooser;
  25   private static SendableChooser frontChooser;
  26   boolean choseIntakeIsFront;
  27
  28   @Override
  29   public void robotInit() {
  30     driveTrain = new DriveTrain();
  31     shooter = new Shooter();
  32     intakeArm = new IntakeArm();
  33     photogate = new Photogate();
  34
  35     oi = new OI();
  36
  37     defenseChooser = new SendableChooser();
  38     addDefenseOptions(defenseChooser);
  39     SmartDashboard.putData("Defense Chooser", defenseChooser);
  40
  41     frontChooser = new SendableChooser();
  42     addFrontChooserOptions();
  43     SmartDashboard.putData("Front Chooser", frontChooser);
  44   }
  45
  46   private void addFrontChooserOptions() {
  47     frontChooser.addDefault("Intake is Back", false);
  48     frontChooser.addObject("Intake is True", true);
  49   }
  50
  51   private void addDefenseOptions(SendableChooser chooser) {
  52     chooser.addDefault("Portcullis", Defense.PORTCULLIS);
  53     chooser.addObject("Sally Port", Defense.SALLY_PORT);
  54     chooser.addObject("Rough Terrain", Defense.ROUGH_TERRAIN);
  55     chooser.addObject("Low Bar", Defense.LOW_BAR);
  56     chooser.addObject("Chival De Frise", Defense.CHIVAL_DE_FRISE);
  57     chooser.addObject("Drawbridge", Defense.DRAWBRIDGE);
  58     chooser.addObject("Moat", Defense.MOAT);
  59     chooser.addObject("Rock Wall", Defense.ROCK_WALL);
  60     chooser.addObject("No Auton", Defense.NONE);
  61   }
  62
  63   @Override
  64   public void autonomousInit() {
  65     Defense chosenDefense = (Defense) (defenseChooser.getSelected());
  66     choseIntakeIsFront = (boolean) frontChooser.getSelected();
  67
  68     // if the shooter is not the front (intake is front) - toggle the drivetrain
  69     if (choseIntakeIsFront)
  70       driveTrain.toggleFlipped();
  71
  72     Scheduler.getInstance().add(new ChooseStrategy(chosenDefense));
  73
  74     // Scheduler.getInstance().add(new TimeDrive(.6, 4));
  75
  76   }
  77
  78   @Override
  79   public void autonomousPeriodic() {
  80     Scheduler.getInstance().run();
  81   }
  82
  83   @Override
  84   public void teleopInit() {
  85     // if intake is front, switch back to shooter as front
  86     if (choseIntakeIsFront)
  87       driveTrain.toggleFlipped();
  88
  89     Scheduler.getInstance().add(new SetLowGear());
  90   }
  91
  92   @Override
  93   public void teleopPeriodic() {
  94     Scheduler.getInstance().run();
  95   }
  96
  97 }