1 package org
.usfirst
.frc
.team3501
.robot
;
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
;
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
;
16 public class Robot
extends IterativeRobot
{
18 public static DriveTrain driveTrain
;
19 public static Shooter shooter
;
21 public static Scaler scaler
;
22 public static DefenseArm defenseArm
;
23 public static IntakeArm intakeArm
;
25 // Sendable Choosers send a drop down menu to the Smart Dashboard.
26 SendableChooser positionChooser
;
27 SendableChooser positionOneDefense
, positionTwoDefense
, positionThreeDefense
,
28 positionFourDefense
, positionFiveDefense
;
31 private final static double NANOSECONDS_PER_SECOND
= 1000000000;
33 public FirebotGyro gyro
;
35 double initialSpeedNanoseconds
;
36 double finalSpeedNanoseconds
;
37 double initialSpeedSeconds
;
38 double finalSpeedSeconds
;
43 public void robotInit() {
44 driveTrain
= new DriveTrain();
45 gyro
= new FirebotGyro(I2C
.Port
.kOnboard
, (byte) 0x68);
47 shooter
= new Shooter();
48 scaler
= new Scaler();
49 defenseArm
= new DefenseArm();
50 intakeArm
= new IntakeArm();
52 // Sendable Choosers allows the driver to select the position of the robot
53 // and the positions of the defenses from a drop-down menu on the Smart
55 // make the Sendable Choosers
56 initializeSendableChoosers();
57 addPositionChooserOptions();
58 addDefensesToAllDefenseSendableChooosers();
59 sendSendableChoosersToSmartDashboard();
63 private void initializeSendableChoosers() {
64 positionChooser
= new SendableChooser();
65 positionOneDefense
= new SendableChooser();
66 positionTwoDefense
= new SendableChooser();
67 positionThreeDefense
= new SendableChooser();
68 positionFourDefense
= new SendableChooser();
69 positionFiveDefense
= new SendableChooser();
72 private void addPositionChooserOptions() {
73 positionChooser
.addDefault("Position 1", 1);
74 positionChooser
.addObject("Position 2", 2);
75 positionChooser
.addObject("Position 3", 3);
76 positionChooser
.addObject("Position 4", 4);
77 positionChooser
.addObject("Position 5", 5);
80 private void addDefensesToAllDefenseSendableChooosers() {
81 addDefenseOptions(positionOneDefense
);
82 addDefenseOptions(positionTwoDefense
);
83 addDefenseOptions(positionThreeDefense
);
84 addDefenseOptions(positionFourDefense
);
85 addDefenseOptions(positionFiveDefense
);
88 private void addDefenseOptions(SendableChooser chooser
) {
89 chooser
.addDefault("Portcullis", Defense
.PORTCULLIS
);
90 chooser
.addObject("Sally Port", Defense
.SALLY_PORT
);
91 chooser
.addObject("Rough Terrain", Defense
.ROUGH_TERRAIN
);
92 chooser
.addObject("Low Bar", Defense
.LOW_BAR
);
93 chooser
.addObject("Cheval De Frise", Defense
.CHEVAL_DE_FRISE
);
94 chooser
.addObject("Drawbridge", Defense
.DRAWBRIDGE
);
95 chooser
.addObject("Moat", Defense
.MOAT
);
96 chooser
.addObject("Rock Wall", Defense
.ROCK_WALL
);
99 private void sendSendableChoosersToSmartDashboard() {
100 SmartDashboard
.putData("PositionChooser", positionChooser
);
101 SmartDashboard
.putData("Position One Defense Chooser", positionOneDefense
);
102 SmartDashboard
.putData("Position Two Defense Chooser", positionTwoDefense
);
103 SmartDashboard
.putData("Position Three Defense Chooser",
104 positionThreeDefense
);
105 SmartDashboard
.putData("Position Four Defense Chooser",
106 positionFourDefense
);
107 SmartDashboard
.putData("Position Five Defense Chooser",
108 positionFiveDefense
);
109 SmartDashboard
.putData("Position Four Defense Chooser",
110 positionFourDefense
);
111 SmartDashboard
.putData("Position Five Defense Chooser",
112 positionFiveDefense
);
114 shooter
= new Shooter();
118 public void autonomousInit() {
119 Scheduler
.getInstance().run();
121 // get options chosen from drop down menu
122 Integer chosenPosition
= (Integer
) positionChooser
.getSelected();
123 Integer chosenDefense
= 0;
125 switch (chosenPosition
) {
127 chosenDefense
= (Integer
) positionOneDefense
.getSelected();
129 chosenDefense
= (Integer
) positionTwoDefense
.getSelected();
131 chosenDefense
= (Integer
) positionThreeDefense
.getSelected();
133 chosenDefense
= (Integer
) positionFourDefense
.getSelected();
135 chosenDefense
= (Integer
) positionFiveDefense
.getSelected();
138 System
.out
.println("Chosen Position: " + chosenPosition
);
139 System
.out
.println("Chosen Defense: " + chosenDefense
);
143 public void autonomousPeriodic() {
144 Scheduler
.getInstance().run();
148 public void teleopInit() {
149 Robot
.driveTrain
.initializeGyro();
153 public void teleopPeriodic() {
154 Scheduler
.getInstance().run();