7 #include "Ultrasonic.h"
10 #include "DigitalInput.h"
11 #include "DigitalOutput.h"
12 #include "NetworkCommunication/UsageReporting.h"
15 #include "WPIErrors.h"
16 #include "LiveWindow/LiveWindow.h"
18 constexpr
double Ultrasonic::kPingTime;
19 const uint32_t Ultrasonic::kPriority;
20 constexpr
double Ultrasonic::kMaxUltrasonicTime;
21 constexpr
double Ultrasonic::kSpeedOfSoundInchesPerSec;
22 Task Ultrasonic::m_task(
"UltrasonicChecker", (FUNCPTR)UltrasonicChecker);
24 bool Ultrasonic::m_automaticEnabled =
false;
25 SEM_ID Ultrasonic::m_semaphore = 0;
36 void Ultrasonic::UltrasonicChecker()
39 while (m_automaticEnabled)
41 if (u == NULL) u = m_firstSensor;
42 if (u == NULL)
return;
44 u->m_pingChannel->
Pulse(kPingTime);
57 void Ultrasonic::Initialize()
60 bool originalMode = m_automaticEnabled;
61 if (m_semaphore == 0) m_semaphore = semBCreate(SEM_Q_PRIORITY, SEM_FULL);
63 semTake(m_semaphore, WAIT_FOREVER);
65 m_nextSensor = m_firstSensor;
70 m_counter =
new Counter(m_echoChannel);
78 static int instances = 0;
80 nUsageReporting::report(nUsageReporting::kResourceType_Ultrasonic, instances);
98 m_allocatedChannels =
true;
112 if (pingChannel == NULL || echoChannel == NULL)
114 wpi_setWPIError(NullParameter);
117 m_allocatedChannels =
false;
118 m_pingChannel = pingChannel;
119 m_echoChannel = echoChannel;
133 m_allocatedChannels =
false;
134 m_pingChannel = &pingChannel;
135 m_echoChannel = &echoChannel;
153 uint8_t echoModuleNumber, uint32_t echoChannel, DistanceUnit units)
155 m_pingChannel =
new DigitalOutput(pingModuleNumber, pingChannel);
156 m_echoChannel =
new DigitalInput(echoModuleNumber, echoChannel);
157 m_allocatedChannels =
true;
170 bool wasAutomaticMode = m_automaticEnabled;
172 if (m_allocatedChannels)
174 delete m_pingChannel;
175 delete m_echoChannel;
177 wpi_assert(m_firstSensor != NULL);
179 semTake(m_semaphore, WAIT_FOREVER);
181 if (
this == m_firstSensor)
183 m_firstSensor = m_nextSensor;
184 if (m_firstSensor == NULL)
191 wpi_assert(m_firstSensor->m_nextSensor != NULL);
192 for (
Ultrasonic *s = m_firstSensor; s != NULL; s = s->m_nextSensor)
194 if (
this == s->m_nextSensor)
196 s->m_nextSensor = s->m_nextSensor->m_nextSensor;
202 semGive(m_semaphore);
203 if (m_firstSensor != NULL && wasAutomaticMode)
218 if (enabling == m_automaticEnabled)
221 m_automaticEnabled = enabling;
226 for (
Ultrasonic *u = m_firstSensor; u != NULL; u = u->m_nextSensor)
228 u->m_counter->
Reset();
231 wpi_assert(m_task.
Verify() ==
false);
241 for (
Ultrasonic *u = m_firstSensor; u != NULL; u = u->m_nextSensor)
243 u->m_counter->
Reset();
258 wpi_assert(!m_automaticEnabled);
261 m_pingChannel->
Pulse(kPingTime);
271 return m_counter->
Get() > 1;
282 return m_counter->GetPeriod() * kSpeedOfSoundInchesPerSec / 2.0;
306 case Ultrasonic::kInches:
308 case Ultrasonic::kMilliMeters:
336 if (m_table != NULL) {
bool Start(uint32_t arg0=0, uint32_t arg1=0, uint32_t arg2=0, uint32_t arg3=0, uint32_t arg4=0, uint32_t arg5=0, uint32_t arg6=0, uint32_t arg7=0, uint32_t arg8=0, uint32_t arg9=0)
virtual void PutNumber(std::string key, double value)=0
static void SetAutomaticMode(bool enabling)
void AddSensor(const char *subsystem, const char *name, LiveWindowSendable *component)
void StartLiveWindowMode()
static LiveWindow * GetInstance()
void SetMaxPeriod(double maxPeriod)
std::string GetSmartDashboardType()
void SetSemiPeriodMode(bool highSemiPeriod)
void InitTable(ITable *subTable)
DistanceUnit GetDistanceUnits()
void StopLiveWindowMode()
void SetDistanceUnits(DistanceUnit units)
Ultrasonic(DigitalOutput *pingChannel, DigitalInput *echoChannel, DistanceUnit units=kInches)