X-Git-Url: http://challenge-bot.com/repos/?a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2FRobot.java;h=8fd099fb2999081e999638cb08d2e46b9872297f;hb=4d4bcc688036e2dfeb20824d91b8a9972c20ecf4;hp=8d69370ed8702c5db2ab20d5105bfe1fc7f120c0;hpb=a6c9c6278567b8c5ed45c5d06f0483a4b8fd5876;p=3501%2Fstronghold-2016

diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java
index 8d69370e..8fd099fb 100644
--- a/src/org/usfirst/frc/team3501/robot/Robot.java
+++ b/src/org/usfirst/frc/team3501/robot/Robot.java
@@ -1,6 +1,7 @@
 package org.usfirst.frc.team3501.robot;
 
 import org.usfirst.frc.team3501.robot.Constants.Defense;
+import org.usfirst.frc.team3501.robot.sensors.GyroLib;
 import org.usfirst.frc.team3501.robot.subsystems.DefenseArm;
 import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
 import org.usfirst.frc.team3501.robot.subsystems.IntakeArm;
@@ -29,32 +30,22 @@ public class Robot extends IterativeRobot {
       positionFourDefense, positionFiveDefense;
 
   // Gyro stuff
-  private final static double NANOSECONDS_PER_SECOND = 1000000000;
-  short rawValue;
-  public GyroClass gyro;
-
-  double degreesIncreased;
-  double degrees;
+  public GyroLib gyro;
 
   @Override
   public void robotInit() {
-    // driveTrain = new DriveTrain();
-    gyro = new GyroClass(I2C.Port.kOnboard, (byte) 0x68);
-    // oi = new OI();
+    driveTrain = new DriveTrain();
+    oi = new OI();
+    gyro = new GyroLib(I2C.Port.kOnboard, false);
 
     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();
+    initializeSendableChoosers();
+    addPositionChooserOptions();
+    addDefensesToAllDefenseSendableChoosers();
+    sendSendableChoosersToSmartDashboard();
 
   }
 
@@ -75,7 +66,7 @@ public class Robot extends IterativeRobot {
     positionChooser.addObject("Position 5", 5);
   }
 
-  private void addDefensesToAllDefenseSendableChooosers() {
+  private void addDefensesToAllDefenseSendableChoosers() {
     addDefenseOptions(positionOneDefense);
     addDefenseOptions(positionTwoDefense);
     addDefenseOptions(positionThreeDefense);
@@ -122,18 +113,16 @@ public class Robot extends IterativeRobot {
     Integer chosenPosition = (Integer) positionChooser.getSelected();
     Integer chosenDefense = 0;
 
-    switch (chosenPosition) {
-    case 1:
+    if (chosenPosition == 1)
       chosenDefense = (Integer) positionOneDefense.getSelected();
-    case 2:
+    else if (chosenPosition == 2)
       chosenDefense = (Integer) positionTwoDefense.getSelected();
-    case 3:
+    else if (chosenPosition == 3)
       chosenDefense = (Integer) positionThreeDefense.getSelected();
-    case 4:
+    else if (chosenPosition == 4)
       chosenDefense = (Integer) positionFourDefense.getSelected();
-    case 5:
+    else if (chosenPosition == 5)
       chosenDefense = (Integer) positionFiveDefense.getSelected();
-    }
 
     System.out.println("Chosen Position: " + chosenPosition);
     System.out.println("Chosen Defense: " + chosenDefense);
@@ -146,11 +135,17 @@ public class Robot extends IterativeRobot {
 
   @Override
   public void teleopInit() {
+
+    gyro.start();
+
   }
 
   @Override
   public void teleopPeriodic() {
     Scheduler.getInstance().run();
+
+    System.out.println("Degrees: " + gyro.getRotationZ().getAngle());
+
   }
 
 }