Debian Codegraph @

libs/hwdrivers/src/CServoeNeck.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file |
6 | See: https://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See: https://www.mrpt.org/License |
8 +------------------------------------------------------------------------+ */
9
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 //
12 #include <mrpt/comms/net_utils.h>
13 #include <mrpt/core/bits_math.h>
14 #include <mrpt/hwdrivers/CServoeNeck.h>
15 #include <mrpt/serialization/CArchive.h>
16 #include <mrpt/serialization/CMessage.h>
17
18 #include <chrono>
19 #include <cmath>
20 #include <cstdint>
21 #include <thread>
22
23 using namespace mrpt;
24 using namespace mrpt::comms;
25 using namespace mrpt::hwdrivers;
26 using namespace mrpt::serialization;
27 using namespace std::literals;
28
29 CServoeNeck::CServoeNeck() : m_usbSerialNumber("eNeck001"), m_PrevAngles(0)
30 {
31 m_offsets.resize(3, 0);
32 }
33
34 CServoeNeck::~CServoeNeck() = default;
35
36 bool CServoeNeck::queryFirmwareVersion(std::string& out_firmwareVersion)
37 {
38 try
39 {
40 mrpt::serialization::CMessage msg, msgRx;
41
42 // Try to connect to the device:
43 if (!checkConnectionAndConnect()) return false;
44
45 msg.type = 0x10;
46 archiveFrom(*this).sendMessage(msg);
47
48 if (archiveFrom(*this).receiveMessage(msgRx))
49 {
50 msgRx.getContentAsString(out_firmwareVersion);
51 std::this_thread::sleep_for(200ms);
52 return true;
53 }
54 else
55 return false;
56 }
57 catch (...)
58 {
59 Close();
60 return false;
61 }
62 } // end-queryFirmwareVersion
63
64 /*-------------------------------------------------------------
65 angle2RegValue
66 -------------------------------------------------------------*/
67 unsigned int CServoeNeck::angle2RegValue(const double angle /* rad */)
68 {
69 const uint16_t reg = 1250 + (1000 / M_PI) * (angle - M_PI * 0.5); // equation: v = s*(a-a0)
70 // + v0 where s = 450/pi;
71 // a0 = 0 and v0 = 750
72 // cout << "Reg: " << reg << endl;
73 return (reg);
74
75 // return ( (uint16_t)( (m_MaxValue/20)*(-2*angle/M_PI+1.5) ) ); //
76 // equation: v = s*(a-a0) + v0 where s = 450/pi; a0 = 0 and v0 = 750
77 // return ( (uint16_t)( (m_MaxValue/20)*(2*angle/M_PI+1.5) ) ); //
78 // equation: v = s*(a-a0) + v0 where s = 450/pi; a0 = 0 and v0 = 750
79 // return ( (uint16_t)( (900/M_PI)*nangle + 750 ) ); // equation:
80 // v
81 // =
82 // s*(a-a0) + v0 where s = 450/pi; a0 = 0 and v0 = 750
83 } // end-angle2RegValue
84
85 /*-------------------------------------------------------------
86 regValue2angle
87 -------------------------------------------------------------*/
88 double CServoeNeck::regValue2angle(const uint16_t value)
89 {
90 double angle = M_PI * 0.5 + (M_PI / 1000) * (value - 1250);
91 return angle;
92
93 // return ( -M_PI*0.5*(20*value/m_MaxValue-1.5) ); //
94 // equation:
95 // angle
96 // =
97 // (pi/2)*(20*OCR/ICR-1.5)
98 // return ( -M_PI*0.25+(M_PI/1800)*(value-1050) ); //
99 // equation:
100 // angle
101 // =
102 // (pi/2)*(20*OCR/ICR-1.5)
103 // return ( M_PI*0.5*(20*value/m_MaxValue-1.5) ); //
104 // equation:
105 // angle
106 // =
107 // (pi/2)*(20*OCR/ICR-1.5)
108 // return ( (value-750)/(900/M_PI) );
109 } // end-regValue2angle
110
111 /*-------------------------------------------------------------
112 setRegisterValue
113 -------------------------------------------------------------*/
114 bool CServoeNeck::setRegisterValue(const uint16_t value, const uint8_t servo, bool fast)
115 {
116 try
117 {
118 if (!isOpen()) return false;
119
120 mrpt::serialization::CMessage msg, msgRx;
121
122 // Send cmd for setting the value of the register:
123 // ------------------------------------------------
124 if (fast)
125 msg.type = 0x15;
126 else
127 msg.type = 0x11;
128 msg.content.resize(3);
129 msg.content[2] = (uint8_t)value; // Low byte
130 msg.content[1] = (uint8_t)(value >> 8); // High byte
131 msg.content[0] = servo; // Servo number
132
133 archiveFrom(*this).sendMessage(msg);
134 if (!archiveFrom(*this).receiveMessage(msgRx)) return false; // Error
135
136 std::this_thread::sleep_for(200ms);
137 return true;
138 }
139 catch (...)
140 {
141 // Error opening device:
142 Close();
143 return false;
144 }
145
146 } // end-setRegisterValue
147
148 /*-------------------------------------------------------------
149 setRegisterValue
150 -------------------------------------------------------------*/
151 bool CServoeNeck::setRegisterValueAndSpeed(
152 const uint16_t value, const uint8_t servo, const uint16_t speed)
153 {
154 try
155 {
156 if (!isOpen()) return false;
157
158 mrpt::serialization::CMessage msg, msgRx;
159
160 // Send cmd for setting the value of the register:
161 // ------------------------------------------------
162 msg.type = 0x16;
163 msg.content.resize(5);
164 msg.content[4] = (uint8_t)speed; // Low byte of the speed of the servo
165 msg.content[3] = (uint8_t)(speed >> 8); // High byte of the speed of the servo
166 msg.content[2] = (uint8_t)value; // Low byte
167 msg.content[1] = (uint8_t)(value >> 8); // High byte
168 msg.content[0] = servo; // Servo number
169
170 archiveFrom(*this).sendMessage(msg);
171 if (!archiveFrom(*this).receiveMessage(msgRx)) return false; // Error
172
173 std::this_thread::sleep_for(200ms);
174 return true;
175 }
176 catch (...)
177 {
178 // Error opening device:
179 Close();
180 return false;
181 }
182
183 } // end-setRegisterValue
184
185 /*-------------------------------------------------------------
186 getRegisterValue
187 -------------------------------------------------------------*/
188 bool CServoeNeck::getRegisterValue(uint16_t& value, const uint8_t servo)
189 {
190 try
191 {
192 if (!isOpen()) return false;
193
194 mrpt::serialization::CMessage msg, msgRx;
195
196 // Send cmd for obtaining the value of the OCR1A register:
197 // --------------------------------------------------------
198 msg.type = 0x12;
199 msg.content.resize(1);
200 msg.content[0] = servo;
201
202 archiveFrom(*this).sendMessage(msg);
203 if (archiveFrom(*this).receiveMessage(msgRx))
204 {
205 if (msgRx.content.size() != 2) return false;
206
207 value = (msgRx.content[0] << 8) + msgRx.content[1];
208 return true;
209 }
210 else
211 return false;
212
213 return true;
214 }
215 catch (...)
216 {
217 // Error opening device:
218 Close();
219 return false;
220 }
221
222 } // end-getRegisterValue
223
224 /*-------------------------------------------------------------
225 getCurrentAngle
226 -------------------------------------------------------------*/
227 bool CServoeNeck::getCurrentAngle(double& angle, const uint8_t servo)
228 {
229 uint16_t value;
230 if (getRegisterValue(value, servo))
231 {
232 angle = regValue2angle(value);
233 return true;
234 }
235 else
236 return false;
237 } // end-getCurrentAngle
238
239 /*-------------------------------------------------------------
240 setAngle
241 -------------------------------------------------------------*/
242 bool CServoeNeck::setAngle(double angle, const uint8_t servo, bool fast)
243 {
244 // double nangle = -angle;
245
246 // if( nangle < -m_TruncateFactor*M_PI/2 ) nangle =
247 // -m_TruncateFactor*M_PI/2;
248 // if( nangle > m_TruncateFactor*M_PI/2 ) nangle =
249 // m_TruncateFactor*M_PI/2;
250
251 // unsigned int reg = angle2RegValue( nangle );
252
253 // std::cout << "Angle: " << RAD2DEG( nangle ) << " - Reg: " << reg <<
254 // std::endl;
255 // return setRegisterValue( reg, servo );
256
257 if (angle < -m_TruncateFactor * M_PI / 2) angle = -m_TruncateFactor * M_PI / 2;
258 if (angle > m_TruncateFactor * M_PI / 2) angle = m_TruncateFactor * M_PI / 2;
259
260 unsigned int reg = angle2RegValue(m_offsets[servo] + angle);
261
262 return setRegisterValue(reg, servo, fast);
263
264 } // end-getCurrentAngle
265
266 /*-------------------------------------------------------------
267 setAngleAndSpeed
268 -------------------------------------------------------------*/
269 bool CServoeNeck::setAngleAndSpeed(double angle, const uint8_t servo, const uint8_t speed)
270 {
271 // speed in the range 15/s-250/s
272 if (angle < -m_TruncateFactor * M_PI / 2) angle = -m_TruncateFactor * M_PI / 2;
273 if (angle > m_TruncateFactor * M_PI / 2) angle = m_TruncateFactor * M_PI / 2;
274
275 unsigned int reg = angle2RegValue(m_offsets[servo] + angle);
276 uint8_t thisSpeed = speed < 15 ? 15 : speed > 250 ? 250 : speed;
277 auto delSpeed = uint16_t(0.25 * 1000000 / (500 + 1000 * (thisSpeed / 180.0f - 0.5)));
278 // cout << "Speed: " << int(speed) << " -> " << delSpeed << endl;
279 // std::cout << "Angle: " << RAD2DEG( angle ) << " - Reg: " << reg <<
280 // std::endl;
281 return setRegisterValueAndSpeed(reg, servo, delSpeed);
282
283 } // end-getCurrentAngle
284
285 /*-------------------------------------------------------------
286 setAngleWithFilter
287 -------------------------------------------------------------*/
288 bool CServoeNeck::setAngleWithFilter(double angle, const uint8_t servo, bool fast)
289 {
290 double nangle = 0;
291 if (m_PrevAngles.size() == m_NumPrevAngles &&
292 m_NumPrevAngles != 0) // If the deque is full populated
293 m_PrevAngles.erase(m_PrevAngles.begin()); // Erase the first angle of the deque
294
295 m_PrevAngles.push_back(angle); // Push back the new angle
296
297 std::deque<double>::iterator it;
298 for (it = m_PrevAngles.begin(); it != m_PrevAngles.end();
299 ++it) // Sum up all the elements in the deque
300 nangle += *it;
301 nangle /= m_PrevAngles.size(); // Mean angle
302
303 return (setAngle(nangle, servo, fast));
304 }
305
306 /*-------------------------------------------------------------
307 disableServo
308 -------------------------------------------------------------*/
309 bool CServoeNeck::disableServo(const uint8_t servo)
310 {
311 try
312 {
313 if (!isOpen()) return false;
314
315 mrpt::serialization::CMessage msg, msgRx;
316
317 // Send cmd for disabling servo:
318 // ----------------------------
319 msg.type = 0x13;
320 msg.content.resize(1);
321 msg.content[0] = servo; // Servo number
322
323 archiveFrom(*this).sendMessage(msg);
324 if (!archiveFrom(*this).receiveMessage(msgRx)) return false; // Error
325
326 return true;
327 }
328 catch (...)
329 {
330 // Error opening device:
331 Close();
332 return false;
333 }
334
335 } // end-getCurrentAngle
336
337 /*-------------------------------------------------------------
338 enableServo
339 -------------------------------------------------------------*/
340 bool CServoeNeck::enableServo(const uint8_t servo)
341 {
342 try
343 {
344 if (!isOpen()) return false;
345
346 mrpt::serialization::CMessage msg, msgRx;
347
348 // Send cmd for enabling the servo:
349 // --------------------------------
350 msg.type = 0x14;
351 msg.content.resize(1);
352 msg.content[0] = servo; // Servo number
353
354 archiveFrom(*this).sendMessage(msg);
355 if (!archiveFrom(*this).receiveMessage(msgRx)) return false; // Error
356
357 return true;
358 }
359 catch (...)
360 {
361 // Error opening device:
362 Close();
363 return false;
364 }
365
366 } // end-getCurrentAngle
367
368 /*-------------------------------------------------------------
369 Center
370 -------------------------------------------------------------*/
371 bool CServoeNeck::center(const uint8_t servo)
372 {
373 unsigned int value = angle2RegValue(m_offsets[servo]);
374 return setRegisterValue(value, servo);
375 } // end-Center
376
377 /*-------------------------------------------------------------
378 checkConnectionAndConnect
379 -------------------------------------------------------------*/
380 bool CServoeNeck::checkConnectionAndConnect()
381 {
382 if (isOpen()) return true;
383
384 try
385 {
386 OpenBySerialNumber(m_usbSerialNumber);
387 std::this_thread::sleep_for(10ms);
388 Purge();
389 std::this_thread::sleep_for(10ms);
390 SetLatencyTimer(1);
391 SetTimeouts(300, 100);
392 return true;
393 }
394 catch (...)
395 {
396 // Error opening device:
397 Close();
398 return false;
399 }
400 } // end-checkConnectionAndConnect
401
402 /*-------------------------------------------------------------
403 setOffsets
404 -------------------------------------------------------------*/
405 void CServoeNeck::setOffsets(float offset0, float offset1, float offset2)
406 {
407 m_offsets.resize(3);
408 m_offsets[0] = offset0;
409 m_offsets[1] = offset1;
410 m_offsets[2] = offset2;
411 }