Frames

Robot Code Structure

0
1
2
3
4
5
1public class Robot extends TimedRobot {
2 /**
3 * Specifies whether or not the Robot is the competition robot
4 */
5 public static boolean isComp = false;
6 public static boolean stringPotBroken = false;
7
8 public double[][] valuesPID = pathway.valuesPID;
9
10 // DECLARATIONS
11 public static AirCompressor AirCompressor;
12 public static AutoHatchKicker AutoHatchKicker;
13 public static CargoBigBrother CargoBigBrother;
14 public static CargoDrawbridge CargoDrawbridge;
15 public static CargoIntake CargoIntake;
16 public static CargoEscalator CargoEscalator;
17 public static CargoScore CargoScore;
18 public static Chassis Chassis;
19 public static ClimberDeploy ClimberDeploy;
20 public static Constants Constants;
21 public static HatchLauncher HatchLauncher;
22 public static HatchBeak HatchBeak;
23 public static LED LED;
24 public static Lift Lift;
25 public static NerdyPath NerdyPath;
26 public static OI oi;
27 public static Pigeon Pigeon;
28 public static Recorder Recorder;
29 public static RoboWrangler RoboWrangler;
30 public static PowerDistributionPanel PDP;
31 public static Shifter Shifter;
32 public static TRexArms TRexArms;
33 public static Vision Vision;
34
35 public static Command autonomousCommand;
36 SendableChooser<String> autonChooser = new SendableChooser<>();
37
38 public static Trajectory driveForwardT, curveFromToHatchRightT, fromRightLoadJTurnToCargoShipT, jTurnToCargoShipRightT;
39 public static Trajectory driveForwardFile;
40 public static Trajectory testSCurveT;
41 public static Trajectory driveOffRightLvl2ToRightRocketT, driveOffRightLvl1ToBackRightRocketT, driveAwayFromBackRightRocketT;
42 public static Trajectory sideTwoHatchFromRightT;
43
44 public static boolean pathsLoaded = false;
45 public static boolean logger;
46
47 private String chosenAuton;
48 public String mac;
49
50 public static double rampRate = 0.2;
51 public static double autonAngle = 0;
52
53 /**
54 * This function is run when the robot is first started up and should be used
55 * for any initialization code.
56 */
57 @Override
58 public void robotInit() {
59 mac = "xx:xx:xx:xx:xx:xx";
60 // Attempt to get the MAC address of the robot
61 try {
62 NetworkInterface network = NetworkInterface.getByInetAddress(InetAddress.getLocalHost());
63
64 byte[] address = network.getHardwareAddress();
65
66 StringBuilder sb = new StringBuilder();
67 for (int i = 0; i < address.length; i++) {
68 sb.append(String.format("%02X%s", address[i], (i < address.length - 1) ? ":" : ""));
69 }
70 mac = sb.toString();
71 System.out.println(mac);
72 } catch (UnknownHostException e) {
73 System.out.println("Unknown Host Exception - " + e);
74 } catch (SocketException e) {
75 System.out.println("Socket Exception - " + e);
76 }
77 /// Determines what robot we are using
78
79 if (mac.equals("00:80:2F:17:89:85")) {
80 System.out.println("PracticeBot " + mac);
81 isComp = false;
82 } else {
83 // If we are not using PracticeBot, assume we are using CompBot (this also will
84 // cover if there is an error while getting the MAC address)
85 System.out.println("CompBot " + mac);
86 isComp = true;
87 }
88
89 isComp = true;
90
91 // CONSTRUCTORS
92 // Keep above other subsystems, as these have dependencies for other subsystems
93 // to be instantiated first.
94 Constants = new Constants();
95
96 AirCompressor = new AirCompressor();
97 AutoHatchKicker = new AutoHatchKicker();
98 CargoDrawbridge = new CargoDrawbridge();
99 CargoEscalator = new CargoEscalator();
100 CargoIntake = new CargoIntake();
101 CargoScore = new CargoScore();
102 Chassis = new Chassis();
103 ClimberDeploy = new ClimberDeploy();
104 HatchBeak = new HatchBeak();
105 HatchLauncher = new HatchLauncher();
106 LED = new LED();
107 Lift = new Lift();
108 Pigeon = new Pigeon();
109 Recorder = new Recorder();
110 RoboWrangler = new RoboWrangler();
111 // PDP = new PowerDistributionPanel();
112 Pigeon = new Pigeon();
113 Shifter = new Shifter();
114 TRexArms = new TRexArms();
115 Vision = new Vision();
116
117 /*
118 * Keep below other subsystems as these have dependencies for other subsystems
119 * to be instantiated first.
120 */
121
122 NerdyPath = new NerdyPath();
123 CargoBigBrother = new CargoBigBrother();
124
125 // Turn off the Limelight LED if it is on.
126 LED.setColor(Robot.LED.off);
127
128 // Writing a trajectory to a file (keep commented out until needed)
129 // Robot.NerdyPath.writeFile("driveForward184", driveForwardT); //187
130
131 oi = new OI();
132
133 autonChooser.setDefaultOption("Auton Do Nothing", "Default");
134 autonChooser.addOption("Hatch Lvl1 Right - Far Rocket Low - Near Rocket Low", "Hatch Lvl1 Right Far Rocket Low Near Rocket Low");
135 // autonChooser.addOption("Hatch Lvl2 Right - Far Rocket Low - Near Rocket Low", "Hatch Lvl2 Right Far Rocket Low Near Rocket Low");
136 autonChooser.addOption("Hatch Lvl1 Right - Near Rocket Low - Near Rocket Mid", "Hatch Lvl1 Right Near Rocket Low Near Rocket Mid");
137 autonChooser.addOption("Hatch Lvl2 Right - Near Rocket Low - Near Rocket Mid", "Hatch Lvl2 Right Near Rocket Low Near Rocket Mid");
138 autonChooser.addOption("Hatch Lvl1 Right - Near Rocket Low - Far Rocket Low", "Hatch Lvl1 Right Near Rocket Low Far Rocket Low");
139 autonChooser.addOption("Hatch Lvl2 Right - Near Rocket Low - Far Rocket Low", "Hatch Lvl2 Right Near Rocket Low Far Rocket Low");
140 autonChooser.addOption("Hatch Lvl1 Mid - Ship 4 - Ship 5", "Hatch Lvl1 Mid Ship 4 Ship 5");
141
142 autonChooser.addOption("Hatch Lvl1 Left - Near Rocket Low - Near Rocket Mid", "Hatch Lvl1 Left Near Rocket Low Near Rocket Mid");
143 autonChooser.addOption("Hatch Lvl2 Left - Near Rocket Low - Near Rocket Mid", "Hatch Lvl2 Left Near Rocket Low Near Rocket Mid");
144 autonChooser.addOption("Hatch Lvl1 Left - Near Rocket Low - Far Rocket Low", "Hatch Lvl1 Left Near Rocket Low Far Rocket Low");
145 autonChooser.addOption("Hatch Lvl2 Left - Near Rocket Low - Far Rocket Low", "Hatch Lvl2 Left Near Rocket Low Far Rocket Low");
146 autonChooser.addOption("Hatch Lvl1 Mid - Ship 5 - Ship 4", "Hatch Lvl1 Mid Ship 5 Ship 4");
147
148 Robot.Chassis.resetEncoders();
149 Robot.Pigeon.resetPidgey();
150 SmartDashboard.putData("Auto mode", autonChooser);
151 Vision.streamMode(2);
152 Vision.setLEDMode(1);
153 // Hold the current lift position so that the lift doesn't move on startup
154 Robot.Lift.setSetpoint(Robot.Lift.getPosition());
155 // Disable the air compressor so it doesn't run every time we start the robot.
156 // Robot.AirCompressor.disable();
157 Robot.ClimberDeploy.undeployClimber();
158 }
159
160 /**
161 * This function is called every robot packet, no matter the mode. Use this for
162 * items like diagnostics that you want ran during disabled, autonomous,
163 * teleoperated and test.
164 *
165 * <p>
166 * This runs after the mode specific periodic functions, but before LiveWindow
167 * and SmartDashboard integrated updating.
168 */
169 @Override
170 public void robotPeriodic() {
171 SmartDashboard.putBoolean("Logger", logger);
172 if (Robot.Lift.getPosition() < Robot.Lift.minValue || Robot.Lift.getPosition() > Robot.Lift.maxValue) {
173 stringPotBroken = true;
174 } else {
175 stringPotBroken = false;
176 }
177
178
179 /*
180 * Using the auton chooser to load trajectories
181 * Only loads trajectories when it detects a change in the auton chooser on the dashboard
182 */
183 if(!autonChooser.getSelected().equals(chosenAuton)) {
184 chosenAuton = autonChooser.getSelected();
185 pathsLoaded = false;
186 switch(autonChooser.getSelected()) {
187 case "Hatch Lvl1 Right Far Rocket Low Near Rocket Low":
188 driveOffRightLvl1ToBackRightRocketT = pathway.driveOffRightLvl1ToBackRightRocket();
189 driveAwayFromBackRightRocketT = pathway.driveAwayFromBackRightRocket();
190 pathsLoaded = true;
191 break;
192 default:
193 //Don't put anything in here because we don't want the robot to move if we don't have an auton with a pathway selected
194 break;
195 }
196 }
197
198 /* --- Driver Dashboard Items --- */
199 SmartDashboard.putBoolean("Driver/String Pot Broken", stringPotBroken);
200 SmartDashboard.putBoolean("Driver/Trolley Sensor", Robot.CargoBigBrother.cargoTrolleySensor.get());
201 SmartDashboard.putBoolean("Driver/Intake sensor", Robot.CargoBigBrother.cargoIntakeSensor.get());
202 SmartDashboard.putBoolean("Driver/Escalator sensor", !Robot.CargoBigBrother.cargoEscalatorSensor.get());
203 SmartDashboard.putNumber("Driver/Air Pressure (PSI)", Robot.AirCompressor.getPressure());
204 SmartDashboard.putBoolean("is Comp", isComp);
205 SmartDashboard.putNumber("Driver/Neo_LF_Temp",
206 (((Robot.Chassis.neoLeftFrontMotor.getMotorTemperature() * 9) / 5) + 32));
207 SmartDashboard.putNumber("Driver/Neo_LR_Temp",
208 (((Robot.Chassis.neoLeftRearMotor.getMotorTemperature() * 9) / 5) + 32));
209 SmartDashboard.putNumber("Driver/Neo_RF_Temp",
210 (((Robot.Chassis.neoRightFrontMotor.getMotorTemperature() * 9) / 5) + 32));
211 SmartDashboard.putNumber("Driver/Neo_RR_Temp",
212 (((Robot.Chassis.neoRightRearMotor.getMotorTemperature() * 9) / 5) + 32));
213 SmartDashboard.putNumber("Driver/Right_Encoder", Robot.Chassis.getRightPosition());
214 SmartDashboard.putNumber("Driver/Left_Encoder", Robot.Chassis.getLeftPosition());
215 SmartDashboard.putNumber("Driver/Compass_Heading", Robot.Pigeon.getYaw());
216 SmartDashboard.putNumber("Driver/Lift_Position", Robot.Lift.getPosition());
217 SmartDashboard.putBoolean("Driver/Compressor On?", Robot.AirCompressor.status());
218 SmartDashboard.putBoolean("Driver/Auto_Line_Sensor", Robot.Chassis.autoLineSensor.get());
219 SmartDashboard.putBoolean("Driver/Climber Line Sensor", Robot.ClimberDeploy.climberLineSensor.get());
220
221 SmartDashboard.putNumber("Right Distance Inch", (Robot.Chassis.getRightPosition() / 13988) * 20);
222 SmartDashboard.putNumber("Left Distance Inch", (Robot.Chassis.getLeftPosition() / 13988) * 20);
223
224 SmartDashboard.putNumber("Sticky Faults", Robot.Chassis.neoLeftRearMotor.getStickyFaults());
225 SmartDashboard.putNumber("Faults", Robot.Chassis.neoLeftRearMotor.getFaults());
226
227 SmartDashboard.putNumber("Climber phase", Robot.ClimberDeploy.climberPhase);
228 SmartDashboard.putBoolean("Ready to climb", Robot.ClimberDeploy.readyToClimb);
229
230 SmartDashboard.putNumber("autonAngle", autonAngle);
231 SmartDashboard.putBoolean("Paths Loaded", pathsLoaded);
232
233 SmartDashboard.putBoolean("Beaked", Robot.HatchBeak.status());
234
235 }
236
237 /**
238 * This function is called once each time the robot enters Disabled mode. You
239 * can use it to reset any subsystem information you want to clear when the
240 * robot is disabled.
241 */
242 @Override
243 public void disabledInit() {
244 Robot.Chassis.setAllNeoBrakeMode(IdleMode.kCoast);
245 Robot.Vision.setLEDMode(1);
246 logger = false;
247 }
248
249 @Override
250 public void disabledPeriodic() {
251 Scheduler.getInstance().run();
252 }
253
254 /**
255 * This autonomous (along with the chooser code above) shows how to select
256 * between different autonomous modes using the dashboard. The sendable chooser
257 * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
258 * remove all of the chooser code and uncomment the getString code to get the
259 * auto name from the text box below the Gyro
260 *
261 * <p>
262 * You can add additional auto modes by adding additional commands to the
263 * chooser code above (like the commented example) or additional comparisons to
264 * the switch structure below with additional strings & commands.
265 */
266 @Override
267 public void autonomousInit() {
268 //Selects the auton command being run based off of the chosen auton
269 switch(autonChooser.getSelected()) {
270 case "Hatch Lvl1 Right Far Rocket Low Near Rocket Low":
271 autonomousCommand = new CGHatchRightLowFarRocketLowNearRocketLow();
272 break;
273 case "Hatch Lvl2 Right Far Rocket Low Near Rocket Low":
274 autonomousCommand = new CGHatchRightHighFarRocketLowNearRocketLow();
275 break;
276 case "Hatch Lvl1 Right Near Rocket Low Near Rocket Mid":
277 autonomousCommand = new CGHatchRightLowNearRocketLowNearRocketMid();
278 break;
279 case "Hatch Lvl2 Right Near Rocket Low Near Rocket Mid":
280 autonomousCommand = new CGHatchRightHighNearRocketLowNearRocketMid();
281 break;
282 case "Hatch Lvl1 Right Near Rocket Low Far Rocket Low":
283 autonomousCommand = new CGHatchRightLowNearRocketLowFarRocketLow();
284 break;
285 case "Hatch Lvl2 Right Near Rocket Low Far Rocket Low":
286 autonomousCommand = new CGHatchRightHighNearRocketLowFarRocketLow();
287 break;
288 case "Hatch Lvl1 Mid Ship 4 Ship 5":
289 autonomousCommand = new CGHatchMiddleShip4Ship5();
290 break;
291 case "Hatch Lvl1 Left Near Rocket Low Near Rocket Mid":
292 autonomousCommand = new CGHatchLeftLowNearRocketLowNearRocketMid();
293 break;
294 case "Hatch Lvl1 Left Near Rocket Low Far Rocket Low":
295 autonomousCommand = new CGHatchLeftLowNearRocketLowFarRocketLow();
296 break;
297 case "Hatch Lvl2 Left Near Rocket Low Far Rocket Low":
298 autonomousCommand = new CGHatchLeftHighNearRocketLowFarRocketLow();
299 break;
300 case "Hatch Lvl1 Mid Ship 5 Ship 4":
301 autonomousCommand = new CGHatchMiddleShip5Ship4();
302 break;
303 case "Hatch Lvl2 Left Near Rocket Low Near Rocket Mid":
304 autonomousCommand = new CGHatchLeftHighNearRocketLowNearRocketMid();
305 break;
306 default:
307 autonomousCommand = new autoDoNothing();
308 break;
309 }
310 Robot.Lift.setSetpoint(Robot.Lift.getPosition());
311
312 /*
313 * String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
314 * switch(autoSelected) { case "My Auto": autonomousCommand = new
315 * MyAutoCommand(); break; case "Default Auto": default: autonomousCommand = new
316 * ExampleCommand(); break; }
317 */
318
319 // schedule the autonomous command (example)
320 if (autonomousCommand != null) {
321 autonomousCommand.start();
322 }
323 }
324
325 /**
326 * This function is called periodically during autonomous.
327 */
328 @Override
329 public void autonomousPeriodic() {
330 Scheduler.getInstance().run();
331 }
332
333 @Override
334 public void teleopInit() {
335 Robot.Chassis.setAllNeoBrakeMode(IdleMode.kCoast);
336 Robot.Lift.setSetpoint(Robot.Lift.getPosition());
337
338 /*
339 * This makes sure that the autonomous stops running when teleop starts running.
340 * If you want the autonomous to continue until interrupted by another command,
341 * remove this line or comment it out.
342 */
343 if (autonomousCommand != null) {
344 autonomousCommand.cancel();
345 }
346 logger = true;
347 }
348
349 /**
350 * This function is called periodically during operator control.
351 */
352 @Override
353 public void teleopPeriodic() {
354 Scheduler.getInstance().run();
355 }
356
357 /**
358 * This function is called periodically during test mode.
359 */
360 @Override
361 public void testPeriodic() {
362 }
363
364}
365
This is the TimedRobot Class which is the starting point of the Robot Program.