7 #include "DriverStationEnhancedIO.h"
8 #include "NetworkCommunication/UsageReporting.h"
9 #include "Synchronized.h"
10 #include "WPIErrors.h"
18 DriverStationEnhancedIO::DriverStationEnhancedIO()
19 : m_inputValid (false)
20 , m_outputValid (false)
21 , m_configChanged (false)
22 , m_requestEnhancedEnable (false)
24 bzero((
char*)&m_inputData,
sizeof(m_inputData));
25 bzero((
char*)&m_outputData,
sizeof(m_outputData));
26 m_outputData.size =
sizeof(m_outputData) - 1;
27 m_outputData.id = kOutputBlockID;
29 m_outputData.data.fixed_digital_out = 0x3;
30 m_inputDataSemaphore = semMCreate(SEM_Q_PRIORITY | SEM_DELETE_SAFE | SEM_INVERSION_SAFE);
31 m_outputDataSemaphore = semMCreate(SEM_Q_PRIORITY | SEM_DELETE_SAFE | SEM_INVERSION_SAFE);
32 m_encoderOffsets[0] = 0;
33 m_encoderOffsets[1] = 0;
41 DriverStationEnhancedIO::~DriverStationEnhancedIO()
43 semDelete(m_outputDataSemaphore);
44 semDelete(m_inputDataSemaphore);
52 void DriverStationEnhancedIO::UpdateData()
56 status_block_t tempOutputData;
58 if (m_outputValid || m_configChanged || m_requestEnhancedEnable)
60 m_outputData.flags = kStatusValid;
61 if (m_requestEnhancedEnable)
64 m_outputData.flags |= kForceEnhancedMode;
71 m_outputData.flags |= kForceEnhancedMode;
73 m_outputData.flags |= kStatusConfigChanged;
75 overrideIOConfig((
char*)&m_outputData, 5);
77 retVal = getDynamicControlData(kOutputBlockID, (
char*)&tempOutputData,
sizeof(status_block_t), 5);
85 if (IsConfigEqual(tempOutputData, m_outputData))
87 m_configChanged =
false;
96 MergeConfigIntoOutput(tempOutputData, m_outputData);
103 MergeConfigIntoOutput(tempOutputData, m_outputData);
105 m_requestEnhancedEnable =
false;
106 m_outputValid =
true;
110 m_outputValid =
false;
111 m_inputValid =
false;
116 control_block_t tempInputData;
117 retVal = getDynamicControlData(kInputBlockID, (
char*)&tempInputData,
sizeof(control_block_t), 5);
118 if (retVal == 0 && tempInputData.data.api_version == kSupportedAPIVersion)
120 m_inputData = tempInputData;
125 m_outputValid =
false;
126 m_inputValid =
false;
134 void DriverStationEnhancedIO::MergeConfigIntoOutput(
const status_block_t &dsOutputBlock, status_block_t &localCache)
136 localCache.data.digital = (localCache.data.digital & dsOutputBlock.data.digital_oe) |
137 (dsOutputBlock.data.digital & ~dsOutputBlock.data.digital_oe);
138 localCache.data.digital_oe = dsOutputBlock.data.digital_oe;
139 localCache.data.digital_pe = dsOutputBlock.data.digital_pe;
140 localCache.data.pwm_period[0] = dsOutputBlock.data.pwm_period[0];
141 localCache.data.pwm_period[1] = dsOutputBlock.data.pwm_period[1];
142 localCache.data.enables = dsOutputBlock.data.enables;
148 bool DriverStationEnhancedIO::IsConfigEqual(
const status_block_t &dsOutputBlock,
const status_block_t &localCache)
150 if (localCache.data.digital_oe != dsOutputBlock.data.digital_oe)
return false;
151 if ((localCache.data.digital & ~dsOutputBlock.data.digital) !=
152 (dsOutputBlock.data.digital & ~dsOutputBlock.data.digital))
return false;
153 if (localCache.data.digital_pe != dsOutputBlock.data.digital_pe)
return false;
154 if (localCache.data.pwm_period[0] != dsOutputBlock.data.pwm_period[0])
return false;
155 if (localCache.data.pwm_period[1] != dsOutputBlock.data.pwm_period[1])
return false;
156 if (localCache.data.enables != dsOutputBlock.data.enables)
return false;
168 if (channel < 1 || channel > 2)
170 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"channel must be between 1 and 2");
175 wpi_setWPIError(EnhancedIOMissing);
179 static uint8_t reported_mask = 0;
180 if (!(reported_mask & (1 >> channel)))
182 nUsageReporting::report(nUsageReporting::kResourceType_DriverStationEIO, channel, nUsageReporting::kDriverStationEIO_Acceleration);
183 reported_mask |= (1 >> channel);
187 return (m_inputData.data.accel[channel] - kAccelOffset) / kAccelScale;
210 if (channel < 1 || channel > 8)
212 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"channel must be between 1 and 8");
217 wpi_setWPIError(EnhancedIOMissing);
221 static uint16_t reported_mask = 0;
222 if (!(reported_mask & (1 >> channel)))
224 nUsageReporting::report(nUsageReporting::kResourceType_DriverStationEIO, channel, nUsageReporting::kDriverStationEIO_AnalogIn);
225 reported_mask |= (1 >> channel);
229 return m_inputData.data.analog[channel-1] / kAnalogInputResolution;
243 if (channel < 1 || channel > 2)
245 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"channel must be between 1 and 2");
250 wpi_setWPIError(EnhancedIOMissing);
255 return m_outputData.data.dac[channel-1] * kAnalogOutputReference / kAnalogOutputResolution;
273 if (channel < 1 || channel > 2)
275 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"channel must be between 1 and 2");
280 wpi_setWPIError(EnhancedIOMissing);
283 if (value < 0.0) value = 0.0;
284 if (value > kAnalogOutputReference) value = kAnalogOutputReference;
286 static uint8_t reported_mask = 0;
287 if (!(reported_mask & (1 >> channel)))
289 nUsageReporting::report(nUsageReporting::kResourceType_DriverStationEIO, channel, nUsageReporting::kDriverStationEIO_AnalogOut);
290 reported_mask |= (1 >> channel);
294 m_outputData.data.dac[channel-1] = (uint8_t)(value / kAnalogOutputReference * kAnalogOutputResolution);
314 if (channel < 1 || channel > 6)
316 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"channel must be between 1 and 6");
320 static uint8_t reported_mask = 0;
321 if (!(reported_mask & (1 >> channel)))
323 nUsageReporting::report(nUsageReporting::kResourceType_DriverStationEIO, channel, nUsageReporting::kDriverStationEIO_Button);
324 reported_mask |= (1 >> channel);
327 return ((
GetButtons() >> (channel-1)) & 1) != 0;
339 wpi_setWPIError(EnhancedIOMissing);
342 nUsageReporting::report(nUsageReporting::kResourceType_DriverStationEIO, 0, nUsageReporting::kDriverStationEIO_Button);
344 return m_inputData.data.buttons;
355 if (channel < 1 || channel > 8)
357 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"channel must be between 1 and 8");
362 wpi_setWPIError(EnhancedIOMissing);
366 static uint16_t reported_mask = 0;
367 if (!(reported_mask & (1 >> channel)))
369 nUsageReporting::report(nUsageReporting::kResourceType_DriverStationEIO, channel, nUsageReporting::kDriverStationEIO_LED);
370 reported_mask |= (1 >> channel);
375 leds = m_outputData.data.leds;
377 leds &= ~(1 << (channel-1));
378 if (value) leds |= 1 << (channel-1);
380 m_outputData.data.leds = leds;
392 wpi_setWPIError(EnhancedIOMissing);
395 nUsageReporting::report(nUsageReporting::kResourceType_DriverStationEIO, 0, nUsageReporting::kDriverStationEIO_LED);
397 m_outputData.data.leds = value;
408 if (channel < 1 || channel > 16)
410 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"channel must be between 1 and 16");
414 static uint32_t reported_mask = 0;
415 if (!(reported_mask & (1 >> channel)))
417 nUsageReporting::report(nUsageReporting::kResourceType_DriverStationEIO, channel, nUsageReporting::kDriverStationEIO_DigitalIn);
418 reported_mask |= (1 >> channel);
433 wpi_setWPIError(EnhancedIOMissing);
436 nUsageReporting::report(nUsageReporting::kResourceType_DriverStationEIO, 0, nUsageReporting::kDriverStationEIO_DigitalIn);
438 return m_inputData.data.digital;
449 if (channel < 1 || channel > 16)
451 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"channel must be between 1 and 16");
456 wpi_setWPIError(EnhancedIOMissing);
460 static uint32_t reported_mask = 0;
461 if (!(reported_mask & (1 >> channel)))
463 nUsageReporting::report(nUsageReporting::kResourceType_DriverStationEIO, channel, nUsageReporting::kDriverStationEIO_DigitalOut);
464 reported_mask |= (1 >> channel);
470 if (m_outputData.data.digital_oe & (1 << (channel-1)))
472 digital = m_outputData.data.digital;
474 digital &= ~(1 << (channel-1));
475 if (value) digital |= 1 << (channel-1);
477 m_outputData.data.digital = digital;
481 wpi_setWPIError(LineNotOutput);
496 if (channel < 1 || channel > 16)
498 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"channel must be between 1 and 16");
503 m_requestEnhancedEnable =
true;
504 wpi_setWPIError(EnhancedIOMissing);
508 if ((channel >= 1) && (channel <= 4))
510 if (m_outputData.data.pwm_enable & (1 << (channel - 1)))
515 if ((channel >= 15) && (channel <= 16))
517 if (m_outputData.data.comparator_enable & (1 << (channel - 15)))
519 return kAnalogComparator;
522 if (m_outputData.data.digital_oe & (1 << (channel - 1)))
526 if (!(m_outputData.data.digital_pe & (1 << (channel - 1))))
528 return kInputFloating;
530 if (m_outputData.data.digital & (1 << (channel - 1)))
536 return kInputPullDown;
565 if (channel < 1 || channel > 16)
567 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"channel must be between 1 and 16");
570 if (config == kPWM && (channel < 1 || channel > 4))
572 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"channel in PWM mode must be between 1 and 4");
575 if (config == kAnalogComparator && (channel < 15 || channel > 16))
577 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"channel in Analog Comparator mode must be between 15 and 16");
582 m_configChanged =
true;
584 if ((channel >= 1) && (channel <= 4))
588 m_outputData.data.pwm_enable |= 1 << (channel - 1);
589 m_outputData.data.digital &= ~(1 << (channel - 1));
590 m_outputData.data.digital_oe |= 1 << (channel - 1);
591 m_outputData.data.digital_pe &= ~(1 << (channel - 1));
596 m_outputData.data.pwm_enable &= ~(1 << (channel - 1));
599 else if ((channel >= 15) && (channel <= 16))
601 if (config == kAnalogComparator)
603 m_outputData.data.comparator_enable |= 1 << (channel - 15);
604 m_outputData.data.digital &= ~(1 << (channel - 1));
605 m_outputData.data.digital_oe &= ~(1 << (channel - 1));
606 m_outputData.data.digital_pe &= ~(1 << (channel - 1));
611 m_outputData.data.comparator_enable &= ~(1 << (channel - 15));
614 if (config == kInputFloating)
616 m_outputData.data.digital &= ~(1 << (channel - 1));
617 m_outputData.data.digital_oe &= ~(1 << (channel - 1));
618 m_outputData.data.digital_pe &= ~(1 << (channel - 1));
620 else if (config == kInputPullUp)
622 m_outputData.data.digital |= 1 << (channel - 1);
623 m_outputData.data.digital_oe &= ~(1 << (channel - 1));
624 m_outputData.data.digital_pe |= 1 << (channel - 1);
626 else if (config == kInputPullDown)
628 m_outputData.data.digital &= ~(1 << (channel - 1));
629 m_outputData.data.digital_oe &= ~(1 << (channel - 1));
630 m_outputData.data.digital_pe |= 1 << (channel - 1);
632 else if (config == kOutput)
634 m_outputData.data.digital_oe |= 1 << (channel - 1);
635 m_outputData.data.digital_pe &= ~(1 << (channel - 1));
654 if (channels < kPWMChannels1and2 || channels > kPWMChannels3and4)
656 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"channels must be kPWMChannels1and2 or kPWMChannels3and4");
661 m_requestEnhancedEnable =
true;
662 wpi_setWPIError(EnhancedIOMissing);
667 return m_outputData.data.pwm_period[channels] / 24000000.0;
682 if (channels < kPWMChannels1and2 || channels > kPWMChannels3and4)
684 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"channels must be kPWMChannels1and2 or kPWMChannels3and4");
689 double ticks = period * 24000000.0;
693 wpi_setWPIError(EnhancedIOPWMPeriodOutOfRange);
696 else if (ticks < 0.0) ticks = 0.0;
698 double dutyCycles[2];
704 m_outputData.data.pwm_period[channels] = (uint16_t)ticks;
705 m_configChanged =
true;
720 if (channel < 1 || channel > 2)
722 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"channel must be between 1 and 2");
727 wpi_setWPIError(EnhancedIOMissing);
732 return ((m_outputData.data.fixed_digital_out >> (channel-1)) & 1) != 0;
751 if (channel < 1 || channel > 2)
753 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"channel must be between 1 and 2");
758 wpi_setWPIError(EnhancedIOMissing);
762 static uint8_t reported_mask = 0;
763 if (!(reported_mask & (1 >> channel)))
765 nUsageReporting::report(nUsageReporting::kResourceType_DriverStationEIO, channel, nUsageReporting::kDriverStationEIO_FixedDigitalOut);
766 reported_mask |= (1 >> channel);
771 digital = m_outputData.data.fixed_digital_out;
773 digital &= ~(1 << (channel-1));
774 if (value) digital |= 1 << (channel-1);
776 m_outputData.data.fixed_digital_out = digital;
795 if (encoderNumber < 1 || encoderNumber > 2)
797 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"encoderNumber must be between 1 and 2");
802 wpi_setWPIError(EnhancedIOMissing);
806 static uint8_t reported_mask = 0;
807 if (!(reported_mask & (1 >> encoderNumber)))
809 nUsageReporting::report(nUsageReporting::kResourceType_DriverStationEIO, encoderNumber, nUsageReporting::kDriverStationEIO_Encoder);
810 reported_mask |= (1 >> encoderNumber);
814 return m_inputData.data.quad[encoderNumber - 1] - m_encoderOffsets[encoderNumber - 1];
827 if (encoderNumber < 1 || encoderNumber > 2)
829 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"encoderNumber must be between 1 and 2");
834 wpi_setWPIError(EnhancedIOMissing);
839 m_encoderOffsets[encoderNumber - 1] = m_inputData.data.quad[encoderNumber - 1];
853 if (encoderNumber < 1 || encoderNumber > 2)
855 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"encoderNumber must be between 1 and 2");
860 m_requestEnhancedEnable =
true;
861 wpi_setWPIError(EnhancedIOMissing);
866 return ((m_outputData.data.quad_index_enable >> (encoderNumber - 1)) & 1) != 0;
884 if (encoderNumber < 1 || encoderNumber > 2)
886 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"encoderNumber must be between 1 and 2");
891 m_outputData.data.quad_index_enable &= ~(1 << (encoderNumber - 1));
892 if (enable) m_outputData.data.quad_index_enable |= 1 << (encoderNumber - 1);
893 m_configChanged =
true;
905 wpi_setWPIError(EnhancedIOMissing);
909 nUsageReporting::report(nUsageReporting::kResourceType_DriverStationEIO, 1, nUsageReporting::kDriverStationEIO_TouchSlider);
912 uint8_t value = m_inputData.data.capsense_slider;
913 return value == 255 ? -1.0 : value / 254.0;
924 if (channel < 1 || channel > 4)
926 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"channel must be between 1 and 4");
931 wpi_setWPIError(EnhancedIOMissing);
936 return (
double)m_outputData.data.pwm_compare[channel - 1] / (double)m_outputData.data.pwm_period[(channel - 1) >> 1];
950 if (channel < 1 || channel > 4)
952 wpi_setWPIErrorWithContext(ParameterOutOfRange,
"channel must be between 1 and 4");
957 wpi_setWPIError(EnhancedIOMissing);
961 static uint8_t reported_mask = 0;
962 if (!(reported_mask & (1 >> channel)))
964 nUsageReporting::report(nUsageReporting::kResourceType_DriverStationEIO, channel, nUsageReporting::kDriverStationEIO_PWM);
965 reported_mask |= (1 >> channel);
968 if (value > 1.0) value = 1.0;
969 else if (value < 0.0) value = 0.0;
971 m_outputData.data.pwm_compare[channel - 1] = (uint16_t)(value * (
double)m_outputData.data.pwm_period[(channel - 1) >> 1]);
987 m_requestEnhancedEnable =
true;
988 wpi_setWPIError(EnhancedIOMissing);
993 return m_inputData.data.fw_version;
void SetLED(uint32_t channel, bool value)
uint8_t GetFirmwareVersion()
double GetPWMPeriod(tPWMPeriodChannels channels)
void SetEncoderIndexEnable(uint32_t encoderNumber, bool enable)
double GetAnalogOut(uint32_t channel)
void SetLEDs(uint8_t value)
void SetDigitalConfig(uint32_t channel, tDigitalConfig config)
double GetAnalogInRatio(uint32_t channel)
double GetAcceleration(tAccelChannel channel)
double GetPWMOutput(uint32_t channel)
bool GetButton(uint32_t channel)
void SetFixedDigitalOutput(uint32_t channel, bool value)
void SetDigitalOutput(uint32_t channel, bool value)
bool GetDigital(uint32_t channel)
void SetPWMPeriod(tPWMPeriodChannels channels, double period)
double GetAnalogIn(uint32_t channel)
void SetPWMOutput(uint32_t channel, double value)
void SetAnalogOut(uint32_t channel, double value)
void ResetEncoder(uint32_t encoderNumber)
int16_t GetEncoder(uint32_t encoderNumber)
tDigitalConfig GetDigitalConfig(uint32_t channel)
bool GetFixedDigitalOutput(uint32_t channel)
bool GetEncoderIndexEnable(uint32_t encoderNumber)