change position 5 to shoot in middle goal
[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / Robot.java
CommitLineData
38a404b3
KZ
1package org.usfirst.frc.team3501.robot;
2
53d61b3e 3import org.usfirst.frc.team3501.robot.Constants.Defense;
2947642e 4import org.usfirst.frc.team3501.robot.subsystems.DefenseArm;
047383c3 5import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
e47f0862 6import org.usfirst.frc.team3501.robot.subsystems.IntakeArm;
23e5c98d 7import org.usfirst.frc.team3501.robot.subsystems.Photogate;
02cb494e 8import org.usfirst.frc.team3501.robot.subsystems.Scaler;
40348cab
E
9import org.usfirst.frc.team3501.robot.subsystems.Shooter;
10
c97fbce2 11import edu.wpi.first.wpilibj.I2C;
38a404b3
KZ
12import edu.wpi.first.wpilibj.IterativeRobot;
13import edu.wpi.first.wpilibj.command.Scheduler;
3366a59a
ME
14import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
15import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
38a404b3
KZ
16
17public class Robot extends IterativeRobot {
18 public static OI oi;
19 public static DriveTrain driveTrain;
40348cab 20 public static Shooter shooter;
2c9f5abb 21
02cb494e 22 public static Scaler scaler;
c97fbce2 23
a96fa926 24 public static IntakeArm intakeArm;
200caf63 25 public static DefenseArm defenseArm;
38a404b3 26
23e5c98d 27 public static Photogate photogate;
28
88e66b4e 29 // Sendable Choosers send a drop down menu to the Smart Dashboard.
3366a59a 30 SendableChooser positionChooser;
6947c8a4 31 SendableChooser positionOneDefense, positionTwoDefense, positionThreeDefense,
53f43a72 32 positionFourDefense, positionFiveDefense;
3366a59a 33
c97fbce2 34 // Gyro stuff
da6933e2 35 public GyroLib gyro;
c97fbce2 36
38a404b3
KZ
37 @Override
38 public void robotInit() {
9742fe2e
KZ
39 driveTrain = new DriveTrain();
40 oi = new OI();
da6933e2 41 gyro = new GyroLib(I2C.Port.kOnboard, false);
2ac0cc17 42
40348cab 43 shooter = new Shooter();
02cb494e 44 scaler = new Scaler();
a96fa926 45 intakeArm = new IntakeArm();
3366a59a 46
9742fe2e
KZ
47 initializeSendableChoosers();
48 addPositionChooserOptions();
49 addDefensesToAllDefenseSendableChoosers();
50 sendSendableChoosersToSmartDashboard();
51
3366a59a
ME
52 }
53
6947c8a4
ME
54 private void initializeSendableChoosers() {
55 positionChooser = new SendableChooser();
56 positionOneDefense = new SendableChooser();
57 positionTwoDefense = new SendableChooser();
58 positionThreeDefense = new SendableChooser();
59 positionFourDefense = new SendableChooser();
60 positionFiveDefense = new SendableChooser();
61 }
62
29d59f48
ME
63 private void addPositionChooserOptions() {
64 positionChooser.addDefault("Position 1", 1);
65 positionChooser.addObject("Position 2", 2);
66 positionChooser.addObject("Position 3", 3);
67 positionChooser.addObject("Position 4", 4);
68 positionChooser.addObject("Position 5", 5);
69 }
70
9742fe2e 71 private void addDefensesToAllDefenseSendableChoosers() {
29d59f48
ME
72 addDefenseOptions(positionOneDefense);
73 addDefenseOptions(positionTwoDefense);
74 addDefenseOptions(positionThreeDefense);
75 addDefenseOptions(positionFourDefense);
76 addDefenseOptions(positionFiveDefense);
77 }
78
79 private void addDefenseOptions(SendableChooser chooser) {
3366a59a
ME
80 chooser.addDefault("Portcullis", Defense.PORTCULLIS);
81 chooser.addObject("Sally Port", Defense.SALLY_PORT);
82 chooser.addObject("Rough Terrain", Defense.ROUGH_TERRAIN);
83 chooser.addObject("Low Bar", Defense.LOW_BAR);
84 chooser.addObject("Cheval De Frise", Defense.CHEVAL_DE_FRISE);
85 chooser.addObject("Drawbridge", Defense.DRAWBRIDGE);
86 chooser.addObject("Moat", Defense.MOAT);
87 chooser.addObject("Rock Wall", Defense.ROCK_WALL);
38a404b3
KZ
88 }
89
29d59f48
ME
90 private void sendSendableChoosersToSmartDashboard() {
91 SmartDashboard.putData("PositionChooser", positionChooser);
92 SmartDashboard.putData("Position One Defense Chooser", positionOneDefense);
93 SmartDashboard.putData("Position Two Defense Chooser", positionTwoDefense);
94 SmartDashboard.putData("Position Three Defense Chooser",
95 positionThreeDefense);
e23bfd98
E
96 SmartDashboard.putData("Position Four Defense Chooser",
97 positionFourDefense);
98 SmartDashboard.putData("Position Five Defense Chooser",
99 positionFiveDefense);
d06ee698 100
e23bfd98
E
101 SmartDashboard.putData("Position Four Defense Chooser",
102 positionFourDefense);
103 SmartDashboard.putData("Position Five Defense Chooser",
104 positionFiveDefense);
105
106 shooter = new Shooter();
d06ee698 107
29d59f48
ME
108 }
109
38a404b3
KZ
110 @Override
111 public void autonomousInit() {
3366a59a
ME
112 Scheduler.getInstance().run();
113
114 // get options chosen from drop down menu
115 Integer chosenPosition = (Integer) positionChooser.getSelected();
e13d2293 116 Integer chosenDefense = 0;
3366a59a 117
33981335
ME
118 switch (chosenPosition) {
119 case 1:
3366a59a 120 chosenDefense = (Integer) positionOneDefense.getSelected();
33981335 121 case 2:
3366a59a 122 chosenDefense = (Integer) positionTwoDefense.getSelected();
33981335 123 case 3:
3366a59a 124 chosenDefense = (Integer) positionThreeDefense.getSelected();
33981335 125 case 4:
3366a59a 126 chosenDefense = (Integer) positionFourDefense.getSelected();
33981335 127 case 5:
3366a59a 128 chosenDefense = (Integer) positionFiveDefense.getSelected();
33981335 129 }
3366a59a
ME
130
131 System.out.println("Chosen Position: " + chosenPosition);
132 System.out.println("Chosen Defense: " + chosenDefense);
38a404b3
KZ
133 }
134
135 @Override
136 public void autonomousPeriodic() {
137 Scheduler.getInstance().run();
38a404b3
KZ
138 }
139
140 @Override
141 public void teleopInit() {
da6933e2
E
142
143 gyro.start();
144
38a404b3
KZ
145 }
146
147 @Override
148 public void teleopPeriodic() {
149 Scheduler.getInstance().run();
1e039ebd 150
da6933e2
E
151 System.out.println("Degrees: " + gyro.getRotationZ().getAngle());
152
1e039ebd
E
153 }
154
38a404b3 155}