1 package org
.usfirst
.frc
.team3501
.robot
;
3 import org
.usfirst
.frc
.team3501
.robot
.AnotherGyroClass
.Rotation
;
4 import org
.usfirst
.frc
.team3501
.robot
.Constants
.Defense
;
5 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.DefenseArm
;
6 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.DriveTrain
;
7 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.IntakeArm
;
8 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.Scaler
;
9 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.Shooter
;
11 import edu
.wpi
.first
.wpilibj
.I2C
;
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
;
17 public class Robot
extends IterativeRobot
{
19 public static DriveTrain driveTrain
;
20 public static Shooter shooter
;
22 public static Scaler scaler
;
25 public static IntakeArm intakeArm
;
26 public static DefenseArm defenseArm
;
28 // Sendable Choosers send a drop down menu to the Smart Dashboard.
29 SendableChooser positionChooser
;
30 SendableChooser positionOneDefense
, positionTwoDefense
, positionThreeDefense
,
31 positionFourDefense
, positionFiveDefense
;
34 private final static double NANOSECONDS_PER_SECOND
= 1000000000;
36 public GyroClass gyro
;
39 double degreesIncreased
;
45 public void robotInit() {
46 // driveTrain = new DriveTrain();
47 gyro
= new GyroClass(I2C
.Port
.kOnboard
, gyro
.ITG3200_ADDRESS_AD0_LOW
);
50 shooter
= new Shooter();
51 scaler
= new Scaler();
52 intakeArm
= new IntakeArm();
54 // Sendable Choosers allows the driver to select the position of the
56 // and the positions of the defenses from a drop-down menu on the Smart
58 // make the Sendable Choosers
59 initializeSendableChoosers();
60 addPositionChooserOptions();
61 addDefensesToAllDefenseSendableChooosers();
62 sendSendableChoosersToSmartDashboard();
66 private void initializeSendableChoosers() {
67 positionChooser
= new SendableChooser();
68 positionOneDefense
= new SendableChooser();
69 positionTwoDefense
= new SendableChooser();
70 positionThreeDefense
= new SendableChooser();
71 positionFourDefense
= new SendableChooser();
72 positionFiveDefense
= new SendableChooser();
75 private void addPositionChooserOptions() {
76 positionChooser
.addDefault("Position 1", 1);
77 positionChooser
.addObject("Position 2", 2);
78 positionChooser
.addObject("Position 3", 3);
79 positionChooser
.addObject("Position 4", 4);
80 positionChooser
.addObject("Position 5", 5);
83 private void addDefensesToAllDefenseSendableChooosers() {
84 addDefenseOptions(positionOneDefense
);
85 addDefenseOptions(positionTwoDefense
);
86 addDefenseOptions(positionThreeDefense
);
87 addDefenseOptions(positionFourDefense
);
88 addDefenseOptions(positionFiveDefense
);
91 private void addDefenseOptions(SendableChooser chooser
) {
92 chooser
.addDefault("Portcullis", Defense
.PORTCULLIS
);
93 chooser
.addObject("Sally Port", Defense
.SALLY_PORT
);
94 chooser
.addObject("Rough Terrain", Defense
.ROUGH_TERRAIN
);
95 chooser
.addObject("Low Bar", Defense
.LOW_BAR
);
96 chooser
.addObject("Cheval De Frise", Defense
.CHEVAL_DE_FRISE
);
97 chooser
.addObject("Drawbridge", Defense
.DRAWBRIDGE
);
98 chooser
.addObject("Moat", Defense
.MOAT
);
99 chooser
.addObject("Rock Wall", Defense
.ROCK_WALL
);
102 private void sendSendableChoosersToSmartDashboard() {
103 SmartDashboard
.putData("PositionChooser", positionChooser
);
104 SmartDashboard
.putData("Position One Defense Chooser", positionOneDefense
);
105 SmartDashboard
.putData("Position Two Defense Chooser", positionTwoDefense
);
106 SmartDashboard
.putData("Position Three Defense Chooser",
107 positionThreeDefense
);
108 SmartDashboard
.putData("Position Four Defense Chooser",
109 positionFourDefense
);
110 SmartDashboard
.putData("Position Five Defense Chooser",
111 positionFiveDefense
);
113 SmartDashboard
.putData("Position Four Defense Chooser",
114 positionFourDefense
);
115 SmartDashboard
.putData("Position Five Defense Chooser",
116 positionFiveDefense
);
118 shooter
= new Shooter();
123 public void autonomousInit() {
124 Scheduler
.getInstance().run();
126 // get options chosen from drop down menu
127 Integer chosenPosition
= (Integer
) positionChooser
.getSelected();
128 Integer chosenDefense
= 0;
130 switch (chosenPosition
) {
132 chosenDefense
= (Integer
) positionOneDefense
.getSelected();
134 chosenDefense
= (Integer
) positionTwoDefense
.getSelected();
136 chosenDefense
= (Integer
) positionThreeDefense
.getSelected();
138 chosenDefense
= (Integer
) positionFourDefense
.getSelected();
140 chosenDefense
= (Integer
) positionFiveDefense
.getSelected();
143 System
.out
.println("Chosen Position: " + chosenPosition
);
144 System
.out
.println("Chosen Defense: " + chosenDefense
);
148 public void autonomousPeriodic() {
149 Scheduler
.getInstance().run();
153 public void teleopInit() {
157 public void teleopPeriodic() {
158 Scheduler
.getInstance().run();
162 public double getZAxisDegreesPerSeconds() {
163 double rawValue
= (double) gyro
.getRotationZ() / 14.375;
167 public void initializeGyro() {
169 then
= System
.nanoTime() / 1000000000.0;
172 System
.out
.println("Testing Gyro Init");
175 public void addZAxisDegrees() {
176 double degreesRead
= getZAxisDegreesPerSeconds();
177 now
= System
.nanoTime();
178 now
= now
/ (1000000000.0);
179 double differenceInTime
= now
- then
;
181 degreesIncreased
= differenceInTime
* degreesRead
;
184 // + 1.0 is the formula constant
186 degrees
+= degreesIncreased
;
190 public double getDegrees() {