1 package org
.usfirst
.frc
.team3501
.robot
;
3 import org
.usfirst
.frc
.team3501
.robot
.Constants
.Defense
;
4 import org
.usfirst
.frc
.team3501
.robot
.sensors
.GyroLib
;
5 import org
.usfirst
.frc
.team3501
.robot
.sensors
.Photogate
;
6 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.DefenseArm
;
7 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.DriveTrain
;
8 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.IntakeArm
;
9 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.Scaler
;
10 import org
.usfirst
.frc
.team3501
.robot
.subsystems
.Shooter
;
12 import edu
.wpi
.first
.wpilibj
.I2C
;
13 import edu
.wpi
.first
.wpilibj
.IterativeRobot
;
14 import edu
.wpi
.first
.wpilibj
.command
.Scheduler
;
15 import edu
.wpi
.first
.wpilibj
.smartdashboard
.SendableChooser
;
16 import edu
.wpi
.first
.wpilibj
.smartdashboard
.SmartDashboard
;
18 public class Robot
extends IterativeRobot
{
20 public static DriveTrain driveTrain
;
21 public static Shooter shooter
;
23 public static Scaler scaler
;
25 public static IntakeArm intakeArm
;
26 public static DefenseArm defenseArm
;
28 public static Photogate photogate
;
30 // Sendable Choosers send a drop down menu to the Smart Dashboard.
31 SendableChooser positionChooser
;
32 SendableChooser positionOneDefense
, positionTwoDefense
, positionThreeDefense
,
33 positionFourDefense
, positionFiveDefense
;
39 public void robotInit() {
40 driveTrain
= new DriveTrain();
42 gyro
= new GyroLib(I2C
.Port
.kOnboard
, false);
44 shooter
= new Shooter();
45 scaler
= new Scaler();
46 intakeArm
= new IntakeArm();
48 initializeSendableChoosers();
49 addPositionChooserOptions();
50 addDefensesToAllDefenseSendableChoosers();
51 sendSendableChoosersToSmartDashboard();
55 private void initializeSendableChoosers() {
56 positionChooser
= new SendableChooser();
57 positionOneDefense
= new SendableChooser();
58 positionTwoDefense
= new SendableChooser();
59 positionThreeDefense
= new SendableChooser();
60 positionFourDefense
= new SendableChooser();
61 positionFiveDefense
= new SendableChooser();
64 private void addPositionChooserOptions() {
65 positionChooser
.addDefault("Position 1", 1);
66 positionChooser
.addObject("Position 2", 2);
67 positionChooser
.addObject("Position 3", 3);
68 positionChooser
.addObject("Position 4", 4);
69 positionChooser
.addObject("Position 5", 5);
72 private void addDefensesToAllDefenseSendableChoosers() {
73 addDefenseOptions(positionOneDefense
);
74 addDefenseOptions(positionTwoDefense
);
75 addDefenseOptions(positionThreeDefense
);
76 addDefenseOptions(positionFourDefense
);
77 addDefenseOptions(positionFiveDefense
);
80 private void addDefenseOptions(SendableChooser chooser
) {
81 chooser
.addDefault("Portcullis", Defense
.PORTCULLIS
);
82 chooser
.addObject("Sally Port", Defense
.SALLY_PORT
);
83 chooser
.addObject("Rough Terrain", Defense
.ROUGH_TERRAIN
);
84 chooser
.addObject("Low Bar", Defense
.LOW_BAR
);
85 chooser
.addObject("Cheval De Frise", Defense
.CHEVAL_DE_FRISE
);
86 chooser
.addObject("Drawbridge", Defense
.DRAWBRIDGE
);
87 chooser
.addObject("Moat", Defense
.MOAT
);
88 chooser
.addObject("Rock Wall", Defense
.ROCK_WALL
);
91 private void sendSendableChoosersToSmartDashboard() {
92 SmartDashboard
.putData("PositionChooser", positionChooser
);
93 SmartDashboard
.putData("Position One Defense Chooser", positionOneDefense
);
94 SmartDashboard
.putData("Position Two Defense Chooser", positionTwoDefense
);
95 SmartDashboard
.putData("Position Three Defense Chooser",
96 positionThreeDefense
);
97 SmartDashboard
.putData("Position Four Defense Chooser",
99 SmartDashboard
.putData("Position Five Defense Chooser",
100 positionFiveDefense
);
102 SmartDashboard
.putData("Position Four Defense Chooser",
103 positionFourDefense
);
104 SmartDashboard
.putData("Position Five Defense Chooser",
105 positionFiveDefense
);
107 shooter
= new Shooter();
112 public void autonomousInit() {
113 Scheduler
.getInstance().run();
115 // get options chosen from drop down menu
116 Integer chosenPosition
= (Integer
) positionChooser
.getSelected();
117 Integer chosenDefense
= 0;
119 if (chosenPosition
== 1)
120 chosenDefense
= (Integer
) positionOneDefense
.getSelected();
121 else if (chosenPosition
== 2)
122 chosenDefense
= (Integer
) positionTwoDefense
.getSelected();
123 else if (chosenPosition
== 3)
124 chosenDefense
= (Integer
) positionThreeDefense
.getSelected();
125 else if (chosenPosition
== 4)
126 chosenDefense
= (Integer
) positionFourDefense
.getSelected();
127 else if (chosenPosition
== 5)
128 chosenDefense
= (Integer
) positionFiveDefense
.getSelected();
130 System
.out
.println("Chosen Position: " + chosenPosition
);
131 System
.out
.println("Chosen Defense: " + chosenDefense
);
135 public void autonomousPeriodic() {
136 Scheduler
.getInstance().run();
140 public void teleopInit() {
147 public void teleopPeriodic() {
148 Scheduler
.getInstance().run();
150 System
.out
.println("Degrees: " + gyro
.getRotationZ().getAngle());