001 /*----------------------------------------------------------------------------*/
002 /* Copyright (c) FIRST 2008-2012. All Rights Reserved. */
003 /* Open Source Software - may be modified and shared by FRC teams. The code */
004 /* must be accompanied by the FIRST BSD license file in the root directory of */
005 /* the project. */
006 /*----------------------------------------------------------------------------*/
007 package edu.wpi.first.wpilibj;
008
009 import edu.wpi.first.wpilibj.communication.*;
010 import edu.wpi.first.wpilibj.parsing.IInputOutput;
011
012 /**
013 * Provide access to the network communication data to / from the Driver Station.
014 */
015 public class DriverStation implements IInputOutput {
016
017 /**
018 * The size of the user control data
019 */
020 public static final int USER_CONTROL_DATA_SIZE = FRCControl.USER_CONTROL_DATA_SIZE;
021 /**
022 * The size of the user status data
023 */
024 public static final int USER_STATUS_DATA_SIZE = FRCControl.USER_STATUS_DATA_SIZE;
025 /**
026 * Slot for the analog module to read the battery
027 */
028 public static final int kBatterySlot = 1;
029 /**
030 * Analog channel to read the battery
031 */
032 public static final int kBatteryChannel = 8;
033 /**
034 * Number of Joystick Ports
035 */
036 public static final int kJoystickPorts = 4;
037 /**
038 * Number of Joystick Axes
039 */
040 public static final int kJoystickAxes = 6;
041 /**
042 * Convert from raw values to volts
043 */
044 public static final double kDSAnalogInScaling = 5.0 / 1023.0;
045
046 /**
047 * The robot alliance that the robot is a part of
048 */
049 public static class Alliance {
050
051 /** The integer value representing this enumeration. */
052 public final int value;
053 /** The Alliance name. */
054 public final String name;
055 public static final int kRed_val = 0;
056 public static final int kBlue_val = 1;
057 public static final int kInvalid_val = 2;
058 /** alliance: Red */
059 public static final Alliance kRed = new Alliance(kRed_val, "Red");
060 /** alliance: Blue */
061 public static final Alliance kBlue = new Alliance(kBlue_val, "Blue");
062 /** alliance: Invalid */
063 public static final Alliance kInvalid = new Alliance(kInvalid_val, "invalid");
064
065 private Alliance(int value, String name) {
066 this.value = value;
067 this.name = name;
068 }
069 } /* Alliance */
070
071
072 private static class DriverStationTask implements Runnable {
073
074 private DriverStation m_ds;
075
076 DriverStationTask(DriverStation ds) {
077 m_ds = ds;
078 }
079
080 public void run() {
081 m_ds.task();
082 }
083 } /* DriverStationTask */
084
085 private static DriverStation instance = new DriverStation();
086 private FRCCommonControlData m_controlData;
087 private AnalogChannel m_batteryChannel;
088 private Thread m_thread;
089 private final Object m_semaphore;
090 private final Object m_dataSem;
091 private int m_digitalOut;
092 private volatile boolean m_thread_keepalive = true;
093 private final Dashboard m_dashboardDefaultHigh;
094 private final Dashboard m_dashboardDefaultLow;
095 private IDashboard m_dashboardInUseHigh;
096 private IDashboard m_dashboardInUseLow;
097 private int m_updateNumber = 0;
098 private double m_approxMatchTimeOffset = -1.0;
099 private boolean m_userInDisabled = false;
100 private boolean m_userInAutonomous = false;
101 private boolean m_userInTeleop = false;
102 private boolean m_userInTest = false;
103 private boolean m_newControlData;
104 private final Semaphore m_packetDataAvailableSem;
105 private DriverStationEnhancedIO m_enhancedIO = new DriverStationEnhancedIO();
106
107 /**
108 * Gets an instance of the DriverStation
109 *
110 * @return The DriverStation.
111 */
112 public static DriverStation getInstance() {
113 return DriverStation.instance;
114 }
115
116 /**
117 * DriverStation constructor.
118 *
119 * The single DriverStation instance is created statically with the
120 * instance static member variable.
121 */
122 protected DriverStation() {
123 m_controlData = new FRCCommonControlData();
124 m_semaphore = new Object();
125 m_dataSem = new Object();
126
127 m_dashboardInUseHigh = m_dashboardDefaultHigh = new Dashboard(m_semaphore);
128 m_dashboardInUseLow = m_dashboardDefaultLow = new Dashboard(m_semaphore);
129
130 // m_controlData is initialized in constructor FRCCommonControlData.
131
132 m_batteryChannel = new AnalogChannel(kBatterySlot, kBatteryChannel);
133
134 Semaphore.Options options = new Semaphore.Options();
135 options.setPrioritySorted(true);
136 m_packetDataAvailableSem = new Semaphore(options, false);
137 FRCControl.setNewDataSem(m_packetDataAvailableSem);
138
139 m_thread = new Thread(new DriverStationTask(this), "FRCDriverStation");
140 m_thread.setPriority((Thread.NORM_PRIORITY + Thread.MAX_PRIORITY) / 2);
141
142 m_thread.start();
143 }
144
145 /**
146 * Kill the thread
147 */
148 public void release() {
149 m_thread_keepalive = false;
150 }
151
152 /**
153 * Provides the service routine for the DS polling thread.
154 */
155 private void task() {
156 int safetyCounter = 0;
157 while (m_thread_keepalive) {
158 try {
159 m_packetDataAvailableSem.takeForever();
160 synchronized (this) {
161 getData();
162 m_enhancedIO.updateData();
163 setData();
164 }
165 synchronized (m_dataSem) {
166 m_dataSem.notifyAll();
167 }
168 if (++safetyCounter >= 5) {
169 // System.out.println("Checking safety");
170 MotorSafetyHelper.checkMotors();
171 safetyCounter = 0;
172 }
173 if (m_userInDisabled) {
174 FRCControl.observeUserProgramDisabled();
175 }
176 if (m_userInAutonomous) {
177 FRCControl.observeUserProgramAutonomous();
178 }
179 if (m_userInTeleop) {
180 FRCControl.observeUserProgramTeleop();
181 }
182 if (m_userInTest) {
183 FRCControl.observeUserProgramTest();
184 }
185 } catch (SemaphoreException ex) {
186 //
187 }
188 }
189 }
190
191 /**
192 * Wait for new data from the driver station.
193 */
194 public void waitForData() {
195 waitForData(0);
196 }
197
198 /**
199 * Wait for new data or for timeout, which ever comes first. If timeout is
200 * 0, wait for new data only.
201 *
202 * @param timeout The maximum time in milliseconds to wait.
203 */
204 public void waitForData(long timeout) {
205 synchronized (m_dataSem) {
206 try {
207 m_dataSem.wait(timeout);
208 } catch (InterruptedException ex) {
209 }
210 }
211 }
212 private static boolean lastEnabled = false;
213
214 /**
215 * Copy data from the DS task for the user.
216 * If no new data exists, it will just be returned, otherwise
217 * the data will be copied from the DS polling loop.
218 */
219 protected synchronized void getData() {
220 FRCControl.getCommonControlData(m_controlData, Semaphore.WAIT_FOREVER);
221
222 if (!lastEnabled && isEnabled()) {
223 // If starting teleop, assume that autonomous just took up 15 seconds
224 if (isAutonomous()) {
225 m_approxMatchTimeOffset = Timer.getFPGATimestamp();
226 } else {
227 m_approxMatchTimeOffset = Timer.getFPGATimestamp() - 15.0;
228 }
229 } else if (lastEnabled && !isEnabled()) {
230 m_approxMatchTimeOffset = -1.0;
231 }
232 lastEnabled = isEnabled();
233
234 m_newControlData = true;
235 }
236
237 /**
238 * Copy status data from the DS task for the user.
239 * This is used primarily to set digital outputs on the DS.
240 */
241 protected void setData() {
242 synchronized (m_semaphore) {
243 FRCControl.setStatusData(getBatteryVoltage(), m_digitalOut,
244 m_updateNumber,
245 m_dashboardInUseHigh.getBytes(),
246 m_dashboardInUseHigh.getBytesLength(),
247 m_dashboardInUseLow.getBytes(),
248 m_dashboardInUseLow.getBytesLength(),
249 Semaphore.WAIT_FOREVER);
250 m_dashboardInUseHigh.flush();
251 m_dashboardInUseLow.flush();
252 }
253 }
254
255 /**
256 * Read the battery voltage from the specified AnalogChannel.
257 *
258 * This accessor assumes that the battery voltage is being measured
259 * through the voltage divider on an analog breakout.
260 *
261 * @return The battery voltage.
262 */
263 public double getBatteryVoltage() {
264 // The Analog bumper has a voltage divider on the battery source.
265 // Vbatt *--/\/\/\--* Vsample *--/\/\/\--* Gnd
266 // 680 Ohms 1000 Ohms
267 return m_batteryChannel.getAverageVoltage() * (1680.0 / 1000.0);
268 }
269
270 /**
271 * Get the value of the axis on a joystick.
272 * This depends on the mapping of the joystick connected to the specified port.
273 *
274 * @param stick The joystick to read.
275 * @param axis The analog axis value to read from the joystick.
276 * @return The value of the axis on the joystick.
277 */
278 public double getStickAxis(int stick, int axis) {
279 if (axis < 1 || axis > kJoystickAxes) {
280 return 0.0;
281 }
282
283 int value;
284 switch (stick) {
285 case 1:
286 value = m_controlData.stick0Axes[axis - 1];
287 break;
288 case 2:
289 value = m_controlData.stick1Axes[axis - 1];
290 break;
291 case 3:
292 value = m_controlData.stick2Axes[axis - 1];
293 break;
294 case 4:
295 value = m_controlData.stick3Axes[axis - 1];
296 break;
297 default:
298 return 0.0;
299 }
300
301 double result;
302 if (value < 0) {
303 result = ((double) value) / 128.0;
304 } else {
305 result = ((double) value) / 127.0;
306 }
307
308 // wpi_assert(result <= 1.0 && result >= -1.0);
309 if (result > 1.0) {
310 result = 1.0;
311 } else if (result < -1.0) {
312 result = -1.0;
313 }
314 return result;
315 }
316
317 /**
318 * The state of the buttons on the joystick.
319 * 12 buttons (4 msb are unused) from the joystick.
320 *
321 * @param stick The joystick to read.
322 * @return The state of the buttons on the joystick.
323 */
324 public int getStickButtons(final int stick) {
325 switch (stick) {
326 case 1:
327 return m_controlData.stick0Buttons;
328 case 2:
329 return m_controlData.stick1Buttons;
330 case 3:
331 return m_controlData.stick2Buttons;
332 case 4:
333 return m_controlData.stick3Buttons;
334 default:
335 return 0;
336 }
337 }
338
339 /**
340 * Get an analog voltage from the Driver Station.
341 * The analog values are returned as voltage values for the Driver Station analog inputs.
342 * These inputs are typically used for advanced operator interfaces consisting of potentiometers
343 * or resistor networks representing values on a rotary switch.
344 *
345 * @param channel The analog input channel on the driver station to read from. Valid range is 1 - 4.
346 * @return The analog voltage on the input.
347 */
348 public double getAnalogIn(final int channel) {
349 switch (channel) {
350 case 1:
351 return kDSAnalogInScaling * m_controlData.analog1;
352 case 2:
353 return kDSAnalogInScaling * m_controlData.analog2;
354 case 3:
355 return kDSAnalogInScaling * m_controlData.analog3;
356 case 4:
357 return kDSAnalogInScaling * m_controlData.analog4;
358 default:
359 return 0.0;
360 }
361 }
362
363 /**
364 * Get values from the digital inputs on the Driver Station.
365 * Return digital values from the Drivers Station. These values are typically used for buttons
366 * and switches on advanced operator interfaces.
367 * @param channel The digital input to get. Valid range is 1 - 8.
368 * @return The value of the digital input
369 */
370 public boolean getDigitalIn(final int channel) {
371 return ((m_controlData.dsDigitalIn >> (channel - 1)) & 0x1) == 0x1;
372 }
373
374 /**
375 * Set a value for the digital outputs on the Driver Station.
376 *
377 * Control digital outputs on the Drivers Station. These values are typically used for
378 * giving feedback on a custom operator station such as LEDs.
379 *
380 * @param channel The digital output to set. Valid range is 1 - 8.
381 * @param value The state to set the digital output.
382 */
383 public void setDigitalOut(final int channel, final boolean value) {
384 m_digitalOut &= ~(0x1 << (channel - 1));
385 m_digitalOut |= ((value ? 1 : 0) << (channel - 1));
386 }
387
388 /**
389 * Get a value that was set for the digital outputs on the Driver Station.
390 * @param channel The digital ouput to monitor. Valid range is 1 through 8.
391 * @return A digital value being output on the Drivers Station.
392 */
393 public boolean getDigitalOut(final int channel) {
394 return ((m_digitalOut >> (channel - 1)) & 0x1) == 0x1;
395 }
396
397 /**
398 * Gets a value indicating whether the Driver Station requires the
399 * robot to be enabled.
400 *
401 * @return True if the robot is enabled, false otherwise.
402 */
403 public boolean isEnabled() {
404 return m_controlData.enabled();
405 }
406
407 /**
408 * Gets a value indicating whether the Driver Station requires the
409 * robot to be disabled.
410 *
411 * @return True if the robot should be disabled, false otherwise.
412 */
413 public boolean isDisabled() {
414 return !m_controlData.enabled();
415 }
416
417 /**
418 * Gets a value indicating whether the Driver Station requires the
419 * robot to be running in autonomous mode.
420 *
421 * @return True if autonomous mode should be enabled, false otherwise.
422 */
423 public boolean isAutonomous() {
424 return m_controlData.autonomous();
425 }
426
427 /**
428 * Gets a value indicating whether the Driver Station requires the
429 * robot to be running in test mode.
430 * @return True if test mode should be enabled, false otherwise.
431 */
432 public boolean isTest() {
433 return m_controlData.testMode();
434 }
435
436 /**
437 * Gets a value indicating whether the Driver Station requires the
438 * robot to be running in operator-controlled mode.
439 *
440 * @return True if operator-controlled mode should be enabled, false otherwise.
441 */
442 public boolean isOperatorControl() {
443 return !(m_controlData.autonomous() || m_controlData.testMode());
444 }
445
446 /**
447 * Has a new control packet from the driver station arrived since the last time this function was called?
448 * @return True if the control data has been updated since the last call.
449 */
450 public synchronized boolean isNewControlData() {
451 boolean result = m_newControlData;
452 m_newControlData = false;
453 return result;
454 }
455
456 /**
457 * Return the DS packet number.
458 * The packet number is the index of this set of data returned by the driver station.
459 * Each time new data is received, the packet number (included with the sent data) is returned.
460 *
461 * @return The DS packet number.
462 */
463 public int getPacketNumber() {
464 return m_controlData.packetIndex;
465 }
466
467 /**
468 * Get the current alliance from the FMS
469 * @return the current alliance
470 */
471 public Alliance getAlliance() {
472 switch (m_controlData.dsID_Alliance) {
473 case 'R':
474 return Alliance.kRed;
475 case 'B':
476 return Alliance.kBlue;
477 default:
478 return Alliance.kInvalid;
479 }
480 }
481
482 /**
483 * Gets the location of the team's driver station controls.
484 *
485 * @return the location of the team's driver station controls: 1, 2, or 3
486 */
487 public int getLocation() {
488 return m_controlData.dsID_Position - '0';
489 }
490
491 /**
492 * Return the team number that the Driver Station is configured for
493 * @return The team number
494 */
495 public int getTeamNumber() {
496 return m_controlData.teamID;
497 }
498
499 /**
500 * Sets the dashboard packer to use for sending high priority user data to a
501 * dashboard receiver. This can idle or restore the default packer.
502 * (Initializing SmartDashboard sets the high priority packer in use, so
503 * beware that the default packer will then be idle. You can restore the
504 * default high priority packer by calling
505 * {@code setDashboardPackerToUseHigh(getDashboardPackerHigh())}.)
506 *
507 * @param dashboard any kind of IDashboard object
508 */
509 public void setDashboardPackerToUseHigh(IDashboard dashboard) {
510 m_dashboardInUseHigh = dashboard;
511 }
512
513 /**
514 * Gets the default dashboard packer for sending high priority user data to
515 * a dashboard receiver. This instance stays around even after a call to
516 * {@link #setDashboardPackerToUseHigh} changes which packer is in use.
517 *
518 * @return the default Dashboard object; it may be idle
519 */
520 public Dashboard getDashboardPackerHigh() {
521 return m_dashboardDefaultHigh;
522 }
523
524 /**
525 * Gets the dashboard packer that's currently in use for sending high
526 * priority user data to a dashboard receiver. This can be any kind of
527 * IDashboard.
528 *
529 * @return the current IDashboard object
530 */
531 public IDashboard getDashboardPackerInUseHigh() {
532 return m_dashboardInUseHigh;
533 }
534
535 /**
536 * Sets the dashboard packer to use for sending low priority user data to a
537 * dashboard receiver. This can idle or restore the default packer.
538 *
539 * @param dashboard any kind of IDashboard object
540 */
541 public void setDashboardPackerToUseLow(IDashboard dashboard) {
542 m_dashboardInUseLow = dashboard;
543 }
544
545 /**
546 * Gets the default dashboard packer for sending low priority user data to
547 * a dashboard receiver. This instance stays around even after a call to
548 * {@link #setDashboardPackerToUseLow} changes which packer is in use.
549 *
550 * @return the default Dashboard object; it may be idle
551 */
552 public Dashboard getDashboardPackerLow() {
553 return m_dashboardDefaultLow;
554 }
555
556 /**
557 * Gets the dashboard packer that's currently in use for sending low
558 * priority user data to a dashboard receiver. This can be any kind of
559 * IDashboard.
560 *
561 * @return the current IDashboard object
562 */
563 public IDashboard getDashboardPackerInUseLow() {
564 return m_dashboardInUseLow;
565 }
566
567 /**
568 * Gets the status data monitor
569 * @return The status data monitor for use with IDashboard objects which must
570 * send data across the network.
571 */
572 public Object getStatusDataMonitor() {
573 return m_semaphore;
574 }
575
576 /**
577 * Increments the internal update number sent across the network along with
578 * status data.
579 */
580 void incrementUpdateNumber() {
581 synchronized (m_semaphore) {
582 m_updateNumber++;
583 }
584 }
585
586 /**
587 * Is the driver station attached to a Field Management System?
588 * Note: This does not work with the Blue DS.
589 * @return True if the robot is competing on a field being controlled by a Field Management System
590 */
591 public boolean isFMSAttached() {
592 return (m_controlData.control & FRCCommonControlData.FMS_ATTATCHED) > 0;
593 }
594
595 /**
596 * Get the interface to the enhanced IO of the new driver station.
597 * @return An enhanced IO object for the advanced features of the driver
598 * station.
599 */
600 public DriverStationEnhancedIO getEnhancedIO() {
601 return m_enhancedIO;
602 }
603
604 /**
605 * Return the approximate match time
606 * The FMS does not currently send the official match time to the robots
607 * This returns the time since the enable signal sent from the Driver Station
608 * At the beginning of autonomous, the time is reset to 0.0 seconds
609 * At the beginning of teleop, the time is reset to +15.0 seconds
610 * If the robot is disabled, this returns 0.0 seconds
611 * Warning: This is not an official time (so it cannot be used to argue with referees)
612 * @return Match time in seconds since the beginning of autonomous
613 */
614 public double getMatchTime() {
615 if (m_approxMatchTimeOffset < 0.0) {
616 return 0.0;
617 }
618 return Timer.getFPGATimestamp() - m_approxMatchTimeOffset;
619 }
620
621 /** Only to be used to tell the Driver Station what code you claim to be executing
622 * for diagnostic purposes only
623 * @param entering If true, starting disabled code; if false, leaving disabled code */
624 public void InDisabled(boolean entering) {m_userInDisabled=entering;}
625
626 /** Only to be used to tell the Driver Station what code you claim to be executing
627 * for diagnostic purposes only
628 * @param entering If true, starting autonomous code; if false, leaving autonomous code */
629 public void InAutonomous(boolean entering) {m_userInAutonomous=entering;}
630
631 /** Only to be used to tell the Driver Station what code you claim to be executing
632 * for diagnostic purposes only
633 * @param entering If true, starting teleop code; if false, leaving teleop code */
634 public void InOperatorControl(boolean entering) {m_userInTeleop=entering;}
635
636 /** Only to be used to tell the Driver Station what code you claim to be executing
637 * for diagnostic purposes only
638 * @param entering If true, starting test code; if false, leaving test code */
639 public void InTest(boolean entering) {m_userInTeleop = entering; }
640 }