Switch all methods of gyro to Robot class so DriveTrain does not have to be depended...
[3501/stronghold-2016] / 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.subsystems.DefenseArm;
5 import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
6 import org.usfirst.frc.team3501.robot.subsystems.IntakeArm;
7 import org.usfirst.frc.team3501.robot.subsystems.Scaler;
8 import org.usfirst.frc.team3501.robot.subsystems.Shooter;
9
10 import edu.wpi.first.wpilibj.I2C;
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
21 public static Scaler scaler;
22 public static DefenseArm defenseArm;
23 public static IntakeArm intakeArm;
24
25 // Sendable Choosers send a drop down menu to the Smart Dashboard.
26 SendableChooser positionChooser;
27 SendableChooser positionOneDefense, positionTwoDefense, positionThreeDefense,
28 positionFourDefense, positionFiveDefense;
29
30 // Gyro stuff
31 private final static double NANOSECONDS_PER_SECOND = 1000000000;
32 short rawValue;
33 public FirebotGyro gyro;
34
35 double initialSpeedNanoseconds;
36 double finalSpeedNanoseconds;
37 double initialSpeedSeconds;
38 double finalSpeedSeconds;
39 double deltaSpeed;
40 double degrees;
41
42 @Override
43 public void robotInit() {
44 // driveTrain = new DriveTrain();
45 gyro = new FirebotGyro(I2C.Port.kOnboard, (byte) 0x68);
46 // oi = new OI();
47
48 shooter = new Shooter();
49 scaler = new Scaler();
50 defenseArm = new DefenseArm();
51 intakeArm = new IntakeArm();
52
53 // Sendable Choosers allows the driver to select the position of the robot
54 // and the positions of the defenses from a drop-down menu on the Smart
55 // Dashboard
56 // make the Sendable Choosers
57 initializeSendableChoosers();
58 addPositionChooserOptions();
59 addDefensesToAllDefenseSendableChooosers();
60 sendSendableChoosersToSmartDashboard();
61
62 }
63
64 private void initializeSendableChoosers() {
65 positionChooser = new SendableChooser();
66 positionOneDefense = new SendableChooser();
67 positionTwoDefense = new SendableChooser();
68 positionThreeDefense = new SendableChooser();
69 positionFourDefense = new SendableChooser();
70 positionFiveDefense = new SendableChooser();
71 }
72
73 private void addPositionChooserOptions() {
74 positionChooser.addDefault("Position 1", 1);
75 positionChooser.addObject("Position 2", 2);
76 positionChooser.addObject("Position 3", 3);
77 positionChooser.addObject("Position 4", 4);
78 positionChooser.addObject("Position 5", 5);
79 }
80
81 private void addDefensesToAllDefenseSendableChooosers() {
82 addDefenseOptions(positionOneDefense);
83 addDefenseOptions(positionTwoDefense);
84 addDefenseOptions(positionThreeDefense);
85 addDefenseOptions(positionFourDefense);
86 addDefenseOptions(positionFiveDefense);
87 }
88
89 private void addDefenseOptions(SendableChooser chooser) {
90 chooser.addDefault("Portcullis", Defense.PORTCULLIS);
91 chooser.addObject("Sally Port", Defense.SALLY_PORT);
92 chooser.addObject("Rough Terrain", Defense.ROUGH_TERRAIN);
93 chooser.addObject("Low Bar", Defense.LOW_BAR);
94 chooser.addObject("Cheval De Frise", Defense.CHEVAL_DE_FRISE);
95 chooser.addObject("Drawbridge", Defense.DRAWBRIDGE);
96 chooser.addObject("Moat", Defense.MOAT);
97 chooser.addObject("Rock Wall", Defense.ROCK_WALL);
98 }
99
100 private void sendSendableChoosersToSmartDashboard() {
101 SmartDashboard.putData("PositionChooser", positionChooser);
102 SmartDashboard.putData("Position One Defense Chooser", positionOneDefense);
103 SmartDashboard.putData("Position Two Defense Chooser", positionTwoDefense);
104 SmartDashboard.putData("Position Three Defense Chooser",
105 positionThreeDefense);
106 SmartDashboard.putData("Position Four Defense Chooser",
107 positionFourDefense);
108 SmartDashboard.putData("Position Five Defense Chooser",
109 positionFiveDefense);
110 SmartDashboard.putData("Position Four Defense Chooser",
111 positionFourDefense);
112 SmartDashboard.putData("Position Five Defense Chooser",
113 positionFiveDefense);
114
115 shooter = new Shooter();
116 }
117
118 @Override
119 public void autonomousInit() {
120 Scheduler.getInstance().run();
121
122 // get options chosen from drop down menu
123 Integer chosenPosition = (Integer) positionChooser.getSelected();
124 Integer chosenDefense = 0;
125
126 switch (chosenPosition) {
127 case 1:
128 chosenDefense = (Integer) positionOneDefense.getSelected();
129 case 2:
130 chosenDefense = (Integer) positionTwoDefense.getSelected();
131 case 3:
132 chosenDefense = (Integer) positionThreeDefense.getSelected();
133 case 4:
134 chosenDefense = (Integer) positionFourDefense.getSelected();
135 case 5:
136 chosenDefense = (Integer) positionFiveDefense.getSelected();
137 }
138
139 System.out.println("Chosen Position: " + chosenPosition);
140 System.out.println("Chosen Defense: " + chosenDefense);
141 }
142
143 @Override
144 public void autonomousPeriodic() {
145 Scheduler.getInstance().run();
146 }
147
148 @Override
149 public void teleopInit() {
150 }
151
152 @Override
153 public void teleopPeriodic() {
154 Scheduler.getInstance().run();
155 }
156 }