Fix instatiation bug by creating OI after the subsystems it uses

[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / Robot.java

diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java
index 9d97660caace18d282b4bef5fdf54e6f2503b47c..7678a7289cc4506c66670c42418b5292fa9a0ef5 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Robot.java
+++ b/src/org/usfirst/frc/team3501/robot/Robot.java
@@ -1,130 +1,54 @@
 package org.usfirst.frc.team3501.robot;
 
-import org.usfirst.frc.team3501.robot.Constants.Defense;
+import org.usfirst.frc.team3501.robot.commands.driving.SetLowGear;
+import org.usfirst.frc.team3501.robot.commands.intakearm.MoveIntakeArm;
+import org.usfirst.frc.team3501.robot.commands.shooter.ResetCatapult;
 import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
 import org.usfirst.frc.team3501.robot.subsystems.IntakeArm;
-import org.usfirst.frc.team3501.robot.subsystems.Scaler;
 import org.usfirst.frc.team3501.robot.subsystems.Shooter;
 
 import edu.wpi.first.wpilibj.IterativeRobot;
 import edu.wpi.first.wpilibj.command.Scheduler;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class Robot extends IterativeRobot {
-       public static OI oi;
-       public static DriveTrain driveTrain;
-       public static Shooter shooter;
-       public static Scaler scaler;
-       public static IntakeArm intakeArm;
+  public static OI oi;
+  public static DriveTrain driveTrain;
+  public static Shooter shooter;
+  public static IntakeArm intakeArm;
+
+  @Override
+  public void robotInit() {
+    driveTrain = new DriveTrain();
+    shooter = new Shooter();
+    intakeArm = new IntakeArm();
+
+    oi = new OI();
+  }
+
+  @Override
+  public void autonomousInit() {
+    Scheduler.getInstance().run();
+  }
+
+  @Override
+  public void autonomousPeriodic() {
+    Scheduler.getInstance().run();
+  }
+
+  @Override
+  public void teleopInit() {
+    Scheduler.getInstance().add(new SetLowGear()); // Start each match in low
+                                                   // gear
+    Scheduler.getInstance().add(new ResetCatapult()); // Reset catapult at start
+                                                      // of each match.
+
+    Scheduler.getInstance().add(new MoveIntakeArm(Constants.IntakeArm.EXTEND));
+    // Start testing with intake arm extended TODO remove this
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    Scheduler.getInstance().run();
+  }
 
-       // Sendable Choosers send a drop down menu to the Smart Dashboard.
-       SendableChooser positionChooser;
-       SendableChooser positionOneDefense, positionTwoDefense, positionThreeDefense, positionFourDefense,
-                       positionFiveDefense;
-
-       @Override
-       public void robotInit() {
-               driveTrain = new DriveTrain();
-               oi = new OI();
-               shooter = new Shooter();
-               scaler = new Scaler();
-               intakeArm = new IntakeArm();
-
-               // Sendable Choosers allows the driver to select the position of the
-               // robot
-               // and the positions of the defenses from a drop-down menu on the Smart
-               // Dashboard
-               // make the Sendable Choosers
-               initializeSendableChoosers();
-               addPositionChooserOptions();
-               addDefensesToAllDefenseSendableChooosers();
-               sendSendableChoosersToSmartDashboard();
-
-       }
-
-       private void initializeSendableChoosers() {
-               positionChooser = new SendableChooser();
-               positionOneDefense = new SendableChooser();
-               positionTwoDefense = new SendableChooser();
-               positionThreeDefense = new SendableChooser();
-               positionFourDefense = new SendableChooser();
-               positionFiveDefense = new SendableChooser();
-       }
-
-       private void addPositionChooserOptions() {
-               positionChooser.addDefault("Position 1", 1);
-               positionChooser.addObject("Position 2", 2);
-               positionChooser.addObject("Position 3", 3);
-               positionChooser.addObject("Position 4", 4);
-               positionChooser.addObject("Position 5", 5);
-       }
-
-       private void addDefensesToAllDefenseSendableChooosers() {
-               addDefenseOptions(positionOneDefense);
-               addDefenseOptions(positionTwoDefense);
-               addDefenseOptions(positionThreeDefense);
-               addDefenseOptions(positionFourDefense);
-               addDefenseOptions(positionFiveDefense);
-       }
-
-       private void addDefenseOptions(SendableChooser chooser) {
-               chooser.addDefault("Portcullis", Defense.PORTCULLIS);
-               chooser.addObject("Sally Port", Defense.SALLY_PORT);
-               chooser.addObject("Rough Terrain", Defense.ROUGH_TERRAIN);
-               chooser.addObject("Low Bar", Defense.LOW_BAR);
-               chooser.addObject("Cheval De Frise", Defense.CHEVAL_DE_FRISE);
-               chooser.addObject("Drawbridge", Defense.DRAWBRIDGE);
-               chooser.addObject("Moat", Defense.MOAT);
-               chooser.addObject("Rock Wall", Defense.ROCK_WALL);
-       }
-
-       private void sendSendableChoosersToSmartDashboard() {
-               SmartDashboard.putData("PositionChooser", positionChooser);
-               SmartDashboard.putData("Position One Defense Chooser", positionOneDefense);
-               SmartDashboard.putData("Position Two Defense Chooser", positionTwoDefense);
-               SmartDashboard.putData("Position Three Defense Chooser", positionThreeDefense);
-               SmartDashboard.putData("Position Four Defense Chooser", positionFourDefense);
-               SmartDashboard.putData("Position Five Defense Chooser", positionFiveDefense);
-       }
-
-       @Override
-       public void autonomousInit() {
-               Scheduler.getInstance().run();
-
-               // get options chosen from drop down menu
-               Integer chosenPosition = (Integer) positionChooser.getSelected();
-               Integer chosenDefense = 0;
-
-               switch (chosenPosition) {
-               case 1:
-                       chosenDefense = (Integer) positionOneDefense.getSelected();
-               case 2:
-                       chosenDefense = (Integer) positionTwoDefense.getSelected();
-               case 3:
-                       chosenDefense = (Integer) positionThreeDefense.getSelected();
-               case 4:
-                       chosenDefense = (Integer) positionFourDefense.getSelected();
-               case 5:
-                       chosenDefense = (Integer) positionFiveDefense.getSelected();
-               }
-
-               System.out.println("Chosen Position: " + chosenPosition);
-               System.out.println("Chosen Defense: " + chosenDefense);
-       }
-
-       @Override
-       public void autonomousPeriodic() {
-               Scheduler.getInstance().run();
-       }
-
-       @Override
-       public void teleopInit() {
-       }
-
-       @Override
-       public void teleopPeriodic() {
-               Scheduler.getInstance().run();
-
-       }
 }