1 package org
.usfirst
.frc
.team3501
.robot
;
3 import org
.usfirst
.frc
.team3501
.robot
.Constants
.Defense
;
4 import org
.usfirst
.frc
.team3501
.robot
.commands
.driving
.SetLowGear
;
5 import org
.usfirst
.frc
.team3501
.robot
.commands
.shooter
.ResetCatapult
;
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
.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
;
24 public static IntakeArm intakeArm
;
25 public static DefenseArm defenseArm
;
27 // Sendable Choosers send a drop down menu to the Smart Dashboard.
28 SendableChooser positionChooser
;
29 SendableChooser positionOneDefense
, positionTwoDefense
, positionThreeDefense
,
30 positionFourDefense
, positionFiveDefense
;
33 public void robotInit() {
34 driveTrain
= new DriveTrain();
37 shooter
= new Shooter();
38 scaler
= new Scaler();
39 intakeArm
= new IntakeArm();
41 initializeSendableChoosers();
42 addPositionChooserOptions();
43 addDefensesToAllDefenseSendableChoosers();
44 sendSendableChoosersToSmartDashboard();
48 private void initializeSendableChoosers() {
49 positionChooser
= new SendableChooser();
50 positionOneDefense
= new SendableChooser();
51 positionTwoDefense
= new SendableChooser();
52 positionThreeDefense
= new SendableChooser();
53 positionFourDefense
= new SendableChooser();
54 positionFiveDefense
= new SendableChooser();
57 private void addPositionChooserOptions() {
58 positionChooser
.addDefault("Position 1", 1);
59 positionChooser
.addObject("Position 2", 2);
60 positionChooser
.addObject("Position 3", 3);
61 positionChooser
.addObject("Position 4", 4);
62 positionChooser
.addObject("Position 5", 5);
65 private void addDefensesToAllDefenseSendableChoosers() {
66 addDefenseOptions(positionOneDefense
);
67 addDefenseOptions(positionTwoDefense
);
68 addDefenseOptions(positionThreeDefense
);
69 addDefenseOptions(positionFourDefense
);
70 addDefenseOptions(positionFiveDefense
);
73 private void addDefenseOptions(SendableChooser chooser
) {
74 chooser
.addDefault("Portcullis", Defense
.PORTCULLIS
);
75 chooser
.addObject("Sally Port", Defense
.SALLY_PORT
);
76 chooser
.addObject("Rough Terrain", Defense
.ROUGH_TERRAIN
);
77 chooser
.addObject("Low Bar", Defense
.LOW_BAR
);
78 chooser
.addObject("Cheval De Frise", Defense
.CHEVAL_DE_FRISE
);
79 chooser
.addObject("Drawbridge", Defense
.DRAWBRIDGE
);
80 chooser
.addObject("Moat", Defense
.MOAT
);
81 chooser
.addObject("Rock Wall", Defense
.ROCK_WALL
);
84 private void sendSendableChoosersToSmartDashboard() {
85 SmartDashboard
.putData("PositionChooser", positionChooser
);
86 SmartDashboard
.putData("Position One Defense Chooser", positionOneDefense
);
87 SmartDashboard
.putData("Position Two Defense Chooser", positionTwoDefense
);
88 SmartDashboard
.putData("Position Three Defense Chooser",
89 positionThreeDefense
);
91 .putData("Position Four Defense Chooser", positionFourDefense
);
93 .putData("Position Five Defense Chooser", positionFiveDefense
);
96 .putData("Position Four Defense Chooser", positionFourDefense
);
98 .putData("Position Five Defense Chooser", positionFiveDefense
);
100 shooter
= new Shooter();
105 public void autonomousInit() {
106 Scheduler
.getInstance().run();
108 // get options chosen from drop down menu
109 Integer chosenPosition
= (Integer
) positionChooser
.getSelected();
110 Integer chosenDefense
= 0;
112 if (chosenPosition
== 1)
113 chosenDefense
= (Integer
) positionOneDefense
.getSelected();
114 else if (chosenPosition
== 2)
115 chosenDefense
= (Integer
) positionTwoDefense
.getSelected();
116 else if (chosenPosition
== 3)
117 chosenDefense
= (Integer
) positionThreeDefense
.getSelected();
118 else if (chosenPosition
== 4)
119 chosenDefense
= (Integer
) positionFourDefense
.getSelected();
120 else if (chosenPosition
== 5)
121 chosenDefense
= (Integer
) positionFiveDefense
.getSelected();
123 System
.out
.println("Chosen Position: " + chosenPosition
);
124 System
.out
.println("Chosen Defense: " + chosenDefense
);
128 public void autonomousPeriodic() {
129 Scheduler
.getInstance().run();
133 public void teleopInit() {
134 Scheduler
.getInstance().add(new SetLowGear()); // Start each match in low
136 Scheduler
.getInstance().add(new ResetCatapult()); // Reset catapult at start
141 public void teleopPeriodic() {
142 Scheduler
.getInstance().run();