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.Auton;
   4 import org.usfirst.frc.team3501.robot.Constants.Defense;
   5 import org.usfirst.frc.team3501.robot.commands.auton.ChooseStrategy;
   6 import org.usfirst.frc.team3501.robot.commands.driving.SetLowGear;
   7 import org.usfirst.frc.team3501.robot.commands.intakearm.Photogate;
   8 import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
   9 import org.usfirst.frc.team3501.robot.subsystems.IntakeArm;
  10 import org.usfirst.frc.team3501.robot.subsystems.Shooter;
  11
  12 import edu.wpi.first.wpilibj.IterativeRobot;
  13 import edu.wpi.first.wpilibj.command.Scheduler;
  14 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
  15 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
  16
  17 public class Robot extends IterativeRobot {
  18   public static OI oi;
  19   public static DriveTrain driveTrain;
  20   public static Shooter shooter;
  21   public static IntakeArm intakeArm;
  22   public static Photogate photogate;
  23
  24   // Sendable Choosers send a drop down menu to the Smart Dashboard.
  25   SendableChooser positionChooser;
  26   SendableChooser positionOneDefense, positionTwoDefense, positionThreeDefense,
  27       positionFourDefense, positionFiveDefense;
  28
  29   @Override
  30   public void robotInit() {
  31     driveTrain = new DriveTrain();
  32     shooter = new Shooter();
  33     intakeArm = new IntakeArm();
  34
  35     oi = new OI();
  36     photogate = new Photogate();
  37
  38     initializeSendableChoosers();
  39     addPositionChooserOptions();
  40     addDefensesToAllDefenseSendableChoosers();
  41     sendSendableChoosersToSmartDashboard();
  42   }
  43
  44   private void initializeSendableChoosers() {
  45     positionChooser = new SendableChooser();
  46     positionOneDefense = new SendableChooser();
  47     positionTwoDefense = new SendableChooser();
  48     positionThreeDefense = new SendableChooser();
  49     positionFourDefense = new SendableChooser();
  50     positionFiveDefense = new SendableChooser();
  51   }
  52
  53   private void addPositionChooserOptions() {
  54     positionChooser.addDefault("None", 0);
  55     positionChooser.addObject("Position 1", 1);
  56     positionChooser.addObject("Position 2", 2);
  57     positionChooser.addObject("Position 3", 3);
  58     positionChooser.addObject("Position 4", 4);
  59     positionChooser.addObject("Position 5", 5);
  60   }
  61
  62   private void addDefensesToAllDefenseSendableChoosers() {
  63     addDefenseOptions(positionOneDefense);
  64     addDefenseOptions(positionTwoDefense);
  65     addDefenseOptions(positionThreeDefense);
  66     addDefenseOptions(positionFourDefense);
  67     addDefenseOptions(positionFiveDefense);
  68   }
  69
  70   private void addDefenseOptions(SendableChooser chooser) {
  71     chooser.addDefault("Portcullis", Defense.PORTCULLIS);
  72     chooser.addObject("Sally Port", Defense.SALLY_PORT);
  73     chooser.addObject("Rough Terrain" + Auton.ROUGH_TERRAIN_SPEED + " "
  74         + Auton.ROUGH_TERRAIN_TIME, Defense.ROUGH_TERRAIN);
  75     chooser.addObject("Low Bar" + " Will probably work...", Defense.LOW_BAR);
  76     chooser.addObject("Chival De Frise", Defense.CHIVAL_DE_FRISE);
  77     chooser.addObject("Drawbridge", Defense.DRAWBRIDGE);
  78     chooser.addObject("Moat" + Auton.MOAT_SPEED + " " + Auton.MOAT_TIME,
  79         Defense.MOAT);
  80     chooser.addObject("Rock Wall" + Auton.ROCK_WALL_SPEED + " "
  81         + Auton.ROCK_WALL_TIME, Defense.ROCK_WALL);
  82   }
  83
  84   private void sendSendableChoosersToSmartDashboard() {
  85     SmartDashboard.putData("PositionChooser", positionChooser);
  86     SmartDashboard.putData("Position 1 Defense Chooser", positionOneDefense);
  87     SmartDashboard.putData("Position 2 Defense Chooser", positionTwoDefense);
  88     SmartDashboard.putData("Position 3 Defense Chooser", positionThreeDefense);
  89     SmartDashboard.putData("Position 4 Defense Chooser", positionFourDefense);
  90     SmartDashboard.putData("Position 5 Defense Chooser", positionFiveDefense);
  91
  92   }
  93
  94   @Override
  95   public void autonomousInit() {
  96     // get options chosen from drop down menu
  97     Integer chosenPosition = (Integer) positionChooser.getSelected();
  98     Defense chosenDefense = null;
  99
 100     if (chosenPosition == 1)
 101       chosenDefense = (Defense) positionOneDefense.getSelected();
 102     else if (chosenPosition == 2)
 103       chosenDefense = (Defense) positionTwoDefense.getSelected();
 104     else if (chosenPosition == 3)
 105       chosenDefense = (Defense) positionThreeDefense.getSelected();
 106     else if (chosenPosition == 4)
 107       chosenDefense = (Defense) positionFourDefense.getSelected();
 108     else if (chosenPosition == 5)
 109       chosenDefense = (Defense) positionFiveDefense.getSelected();
 110
 111     if (chosenPosition != 0)
 112       Scheduler.getInstance().add(
 113           new ChooseStrategy(chosenPosition, chosenDefense));
 114
 115     // Scheduler.getInstance().add(new TimeDrive(.6, 4));
 116   }
 117
 118   @Override
 119   public void autonomousPeriodic() {
 120     Scheduler.getInstance().run();
 121   }
 122
 123   @Override
 124   public void teleopInit() {
 125     Scheduler.getInstance().add(new SetLowGear());
 126   }
 127
 128   @Override
 129   public void teleopPeriodic() {
 130     Scheduler.getInstance().run();
 131   }
 132
 133 }