Main MRPT website > C++ reference for MRPT 1.9.9
CServoeNeck.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
12 #include <mrpt/comms/net_utils.h>
13 #include <mrpt/hwdrivers/CServoeNeck.h>
14 #include <mrpt/serialization/CMessage.h>
15 #include <mrpt/serialization/CArchive.h>
16 #include <mrpt/core/bits_math.h>
17 #include <thread>
18 #include <cstdint>
19 #include <cmath>
20 
21 using namespace mrpt;
22 using namespace mrpt::comms;
23 using namespace mrpt::hwdrivers;
24 using namespace mrpt::serialization;
25 using namespace std::literals;
26 
27 /*-------------------------------------------------------------
28  default constructor
29 -------------------------------------------------------------*/
30 CServoeNeck::CServoeNeck()
31  : m_usbSerialNumber("eNeck001"),
32  m_MaxValue(10000),
33  m_TruncateFactor(0.5),
34  m_PrevAngles(0),
35  m_NumPrevAngles(5)
36 {
37  m_offsets.resize(3, 0);
38 } // end-constructor
39 /*-------------------------------------------------------------
40  default destructor
41 -------------------------------------------------------------*/
42 CServoeNeck::~CServoeNeck() {}
43 /*-------------------------------------------------------------
44  queryFirmwareVersion
45 -------------------------------------------------------------*/
46 bool CServoeNeck::queryFirmwareVersion(std::string& out_firmwareVersion)
47 {
48  try
49  {
50  mrpt::serialization::CMessage msg, msgRx;
51 
52  // Try to connect to the device:
53  if (!checkConnectionAndConnect()) return false;
54 
55  msg.type = 0x10;
56  archiveFrom(*this).sendMessage(msg);
57 
58  if (archiveFrom(*this).receiveMessage(msgRx))
59  {
60  msgRx.getContentAsString(out_firmwareVersion);
61  std::this_thread::sleep_for(200ms);
62  return true;
63  }
64  else
65  return false;
66  }
67  catch (...)
68  {
69  Close();
70  return false;
71  }
72 } // end-queryFirmwareVersion
73 
74 /*-------------------------------------------------------------
75  angle2RegValue
76 -------------------------------------------------------------*/
77 unsigned int CServoeNeck::angle2RegValue(const double angle /* rad */)
78 {
79  const uint16_t reg =
80  1250 + (1000 / M_PI) * (angle - M_PI * 0.5); // equation: v = s*(a-a0)
81  // + v0 where s = 450/pi;
82  // a0 = 0 and v0 = 750
83  // cout << "Reg: " << reg << endl;
84  return (reg);
85 
86  // return ( (uint16_t)( (m_MaxValue/20)*(-2*angle/M_PI+1.5) ) ); //
87  // equation: v = s*(a-a0) + v0 where s = 450/pi; a0 = 0 and v0 = 750
88  // return ( (uint16_t)( (m_MaxValue/20)*(2*angle/M_PI+1.5) ) ); //
89  // equation: v = s*(a-a0) + v0 where s = 450/pi; a0 = 0 and v0 = 750
90  // return ( (uint16_t)( (900/M_PI)*nangle + 750 ) ); // equation:
91  // v
92  // =
93  // s*(a-a0) + v0 where s = 450/pi; a0 = 0 and v0 = 750
94 } // end-angle2RegValue
95 
96 /*-------------------------------------------------------------
97  regValue2angle
98 -------------------------------------------------------------*/
99 double CServoeNeck::regValue2angle(const uint16_t value)
100 {
101  double angle = M_PI * 0.5 + (M_PI / 1000) * (value - 1250);
102  return angle;
103 
104  // return ( -M_PI*0.5*(20*value/m_MaxValue-1.5) ); //
105  // equation:
106  // angle
107  // =
108  // (pi/2)*(20*OCR/ICR-1.5)
109  // return ( -M_PI*0.25+(M_PI/1800)*(value-1050) ); //
110  // equation:
111  // angle
112  // =
113  // (pi/2)*(20*OCR/ICR-1.5)
114  // return ( M_PI*0.5*(20*value/m_MaxValue-1.5) ); //
115  // equation:
116  // angle
117  // =
118  // (pi/2)*(20*OCR/ICR-1.5)
119  // return ( (value-750)/(900/M_PI) );
120 } // end-regValue2angle
121 
122 /*-------------------------------------------------------------
123  setRegisterValue
124 -------------------------------------------------------------*/
125 bool CServoeNeck::setRegisterValue(
126  const uint16_t value, const uint8_t servo, bool fast)
127 {
128  try
129  {
130  if (!isOpen()) return false;
131 
132  mrpt::serialization::CMessage msg, msgRx;
133 
134  // Send cmd for setting the value of the register:
135  // ------------------------------------------------
136  if (fast)
137  msg.type = 0x15;
138  else
139  msg.type = 0x11;
140  msg.content.resize(3);
141  msg.content[2] = (uint8_t)value; // Low byte
142  msg.content[1] = (uint8_t)(value >> 8); // High byte
143  msg.content[0] = servo; // Servo number
144 
145  archiveFrom(*this).sendMessage(msg);
146  if (!archiveFrom(*this).receiveMessage(msgRx)) return false; // Error
147 
148  std::this_thread::sleep_for(200ms);
149  return true;
150  }
151  catch (...)
152  {
153  // Error opening device:
154  Close();
155  return false;
156  }
157 
158 } // end-setRegisterValue
159 
160 /*-------------------------------------------------------------
161  setRegisterValue
162 -------------------------------------------------------------*/
163 bool CServoeNeck::setRegisterValueAndSpeed(
164  const uint16_t value, const uint8_t servo, const uint16_t speed)
165 {
166  try
167  {
168  if (!isOpen()) return false;
169 
170  mrpt::serialization::CMessage msg, msgRx;
171 
172  // Send cmd for setting the value of the register:
173  // ------------------------------------------------
174  msg.type = 0x16;
175  msg.content.resize(5);
176  msg.content[4] = (uint8_t)speed; // Low byte of the speed of the servo
177  msg.content[3] =
178  (uint8_t)(speed >> 8); // High byte of the speed of the servo
179  msg.content[2] = (uint8_t)value; // Low byte
180  msg.content[1] = (uint8_t)(value >> 8); // High byte
181  msg.content[0] = servo; // Servo number
182 
183  archiveFrom(*this).sendMessage(msg);
184  if (!archiveFrom(*this).receiveMessage(msgRx)) return false; // Error
185 
186  std::this_thread::sleep_for(200ms);
187  return true;
188  }
189  catch (...)
190  {
191  // Error opening device:
192  Close();
193  return false;
194  }
195 
196 } // end-setRegisterValue
197 
198 /*-------------------------------------------------------------
199  getRegisterValue
200 -------------------------------------------------------------*/
201 bool CServoeNeck::getRegisterValue(uint16_t& value, const uint8_t servo)
202 {
203  try
204  {
205  if (!isOpen()) return false;
206 
207  mrpt::serialization::CMessage msg, msgRx;
208 
209  // Send cmd for obtaining the value of the OCR1A register:
210  // --------------------------------------------------------
211  msg.type = 0x12;
212  msg.content.resize(1);
213  msg.content[0] = servo;
214 
215  archiveFrom(*this).sendMessage(msg);
216  if (archiveFrom(*this).receiveMessage(msgRx))
217  {
218  if (msgRx.content.size() != 2) return false;
219 
220  value = (msgRx.content[0] << 8) + msgRx.content[1];
221  return true;
222  }
223  else
224  return false;
225 
226  return true;
227  }
228  catch (...)
229  {
230  // Error opening device:
231  Close();
232  return false;
233  }
234 
235 } // end-getRegisterValue
236 
237 /*-------------------------------------------------------------
238  getCurrentAngle
239 -------------------------------------------------------------*/
240 bool CServoeNeck::getCurrentAngle(double& angle, const uint8_t servo)
241 {
242  uint16_t value;
243  if (getRegisterValue(value, servo))
244  {
245  angle = regValue2angle(value);
246  return true;
247  }
248  else
249  return false;
250 } // end-getCurrentAngle
251 
252 /*-------------------------------------------------------------
253  setAngle
254 -------------------------------------------------------------*/
255 bool CServoeNeck::setAngle(double angle, const uint8_t servo, bool fast)
256 {
257  // double nangle = -angle;
258 
259  // if( nangle < -m_TruncateFactor*M_PI/2 ) nangle =
260  // -m_TruncateFactor*M_PI/2;
261  // if( nangle > m_TruncateFactor*M_PI/2 ) nangle =
262  // m_TruncateFactor*M_PI/2;
263 
264  // unsigned int reg = angle2RegValue( nangle );
265 
266  // std::cout << "Angle: " << RAD2DEG( nangle ) << " - Reg: " << reg <<
267  // std::endl;
268  // return setRegisterValue( reg, servo );
269 
270  if (angle < -m_TruncateFactor * M_PI / 2)
271  angle = -m_TruncateFactor * M_PI / 2;
272  if (angle > m_TruncateFactor * M_PI / 2)
273  angle = m_TruncateFactor * M_PI / 2;
274 
275  unsigned int reg = angle2RegValue(m_offsets[servo] + angle);
276 
277  std::cout << "Angle: " << RAD2DEG(angle) << " - Reg: " << reg << std::endl;
278  return setRegisterValue(reg, servo, fast);
279 
280 } // end-getCurrentAngle
281 
282 /*-------------------------------------------------------------
283  setAngleAndSpeed
284 -------------------------------------------------------------*/
285 bool CServoeNeck::setAngleAndSpeed(
286  double angle, const uint8_t servo, const uint8_t speed)
287 {
288  // speed in the range 15/s-250/s
289  if (angle < -m_TruncateFactor * M_PI / 2)
290  angle = -m_TruncateFactor * M_PI / 2;
291  if (angle > m_TruncateFactor * M_PI / 2)
292  angle = m_TruncateFactor * M_PI / 2;
293 
294  unsigned int reg = angle2RegValue(m_offsets[servo] + angle);
295  uint8_t thisSpeed = speed < 15 ? 15 : speed > 250 ? 250 : speed;
296  uint16_t delSpeed =
297  uint16_t(0.25 * 1000000 / (500 + 1000 * (thisSpeed / 180.0f - 0.5)));
298  // cout << "Speed: " << int(speed) << " -> " << delSpeed << endl;
299  // std::cout << "Angle: " << RAD2DEG( angle ) << " - Reg: " << reg <<
300  // std::endl;
301  return setRegisterValueAndSpeed(reg, servo, delSpeed);
302 
303 } // end-getCurrentAngle
304 
305 /*-------------------------------------------------------------
306  setAngleWithFilter
307 -------------------------------------------------------------*/
308 bool CServoeNeck::setAngleWithFilter(
309  double angle, const uint8_t servo, bool fast)
310 {
311  double nangle = 0;
312  if (m_PrevAngles.size() == m_NumPrevAngles &&
313  m_NumPrevAngles != 0) // If the deque is full populated
314  m_PrevAngles.erase(
315  m_PrevAngles.begin()); // Erase the first angle of the deque
316 
317  m_PrevAngles.push_back(angle); // Push back the new angle
318 
319  std::deque<double>::iterator it;
320  for (it = m_PrevAngles.begin(); it != m_PrevAngles.end();
321  ++it) // Sum up all the elements in the deque
322  nangle += *it;
323  nangle /= m_PrevAngles.size(); // Mean angle
324 
325  return (setAngle(nangle, servo, fast));
326 }
327 
328 /*-------------------------------------------------------------
329  disableServo
330 -------------------------------------------------------------*/
331 bool CServoeNeck::disableServo(const uint8_t servo)
332 {
333  try
334  {
335  if (!isOpen()) return false;
336 
337  mrpt::serialization::CMessage msg, msgRx;
338 
339  // Send cmd for disabling servo:
340  // ----------------------------
341  msg.type = 0x13;
342  msg.content.resize(1);
343  msg.content[0] = servo; // Servo number
344 
345  archiveFrom(*this).sendMessage(msg);
346  if (!archiveFrom(*this).receiveMessage(msgRx)) return false; // Error
347 
348  return true;
349  }
350  catch (...)
351  {
352  // Error opening device:
353  Close();
354  return false;
355  }
356 
357 } // end-getCurrentAngle
358 
359 /*-------------------------------------------------------------
360  enableServo
361 -------------------------------------------------------------*/
362 bool CServoeNeck::enableServo(const uint8_t servo)
363 {
364  try
365  {
366  if (!isOpen()) return false;
367 
368  mrpt::serialization::CMessage msg, msgRx;
369 
370  // Send cmd for enabling the servo:
371  // --------------------------------
372  msg.type = 0x14;
373  msg.content.resize(1);
374  msg.content[0] = servo; // Servo number
375 
376  archiveFrom(*this).sendMessage(msg);
377  if (!archiveFrom(*this).receiveMessage(msgRx)) return false; // Error
378 
379  return true;
380  }
381  catch (...)
382  {
383  // Error opening device:
384  Close();
385  return false;
386  }
387 
388 } // end-getCurrentAngle
389 
390 /*-------------------------------------------------------------
391  Center
392 -------------------------------------------------------------*/
393 bool CServoeNeck::center(const uint8_t servo)
394 {
395  unsigned int value = angle2RegValue(m_offsets[servo]);
396  return setRegisterValue(value, servo);
397 } // end-Center
398 
399 /*-------------------------------------------------------------
400  checkConnectionAndConnect
401 -------------------------------------------------------------*/
402 bool CServoeNeck::checkConnectionAndConnect()
403 {
404  if (isOpen()) return true;
405 
406  try
407  {
408  OpenBySerialNumber(m_usbSerialNumber);
409  std::this_thread::sleep_for(10ms);
410  Purge();
411  std::this_thread::sleep_for(10ms);
412  SetLatencyTimer(1);
413  SetTimeouts(300, 100);
414  return true;
415  }
416  catch (...)
417  {
418  // Error opening device:
419  Close();
420  return false;
421  }
422 } // end-checkConnectionAndConnect
423 
424 /*-------------------------------------------------------------
425  setOffsets
426 -------------------------------------------------------------*/
427 void CServoeNeck::setOffsets(float offset0, float offset1, float offset2)
428 {
429  m_offsets.resize(3);
430  m_offsets[0] = offset0;
431  m_offsets[1] = offset1;
432  m_offsets[2] = offset2;
433 }
mrpt::hwdrivers::CServoeNeck::checkConnectionAndConnect
bool checkConnectionAndConnect()
Tries to connect to the USB device (if disconnected).
Definition: CServoeNeck.cpp:402
mrpt::comms::CInterfaceFTDI::Purge
void Purge()
Purge the I/O buffers.
Definition: CInterfaceFTDI_WIN.cpp:337
uint16_t
unsigned __int16 uint16_t
Definition: rptypes.h:44
mrpt::hwdrivers::CServoeNeck::m_NumPrevAngles
unsigned int m_NumPrevAngles
Number of previous angles to store for averaging.
Definition: CServoeNeck.h:131
mrpt::hwdrivers::CServoeNeck::queryFirmwareVersion
bool queryFirmwareVersion(std::string &out_firmwareVersion)
Gets the firmware version of the eNeck board.
Definition: CServoeNeck.cpp:46
mrpt::hwdrivers::CServoeNeck::setRegisterValueAndSpeed
bool setRegisterValueAndSpeed(const uint16_t value, const uint8_t servo, const uint16_t speed)
Definition: CServoeNeck.cpp:163
mrpt::hwdrivers::CServoeNeck::angle2RegValue
unsigned int angle2RegValue(const double angle)
Converts from a decimal angle (in radians) to the corresponding register value for the ATMEGA16 contr...
Definition: CServoeNeck.cpp:77
mrpt::comms::CInterfaceFTDI::isOpen
bool isOpen()
Checks whether the chip has been successfully open.
Definition: CInterfaceFTDI_WIN.cpp:179
mrpt::hwdrivers::CServoeNeck::center
bool center(const uint8_t servo=0)
Centers the servo at zero position.
Definition: CServoeNeck.cpp:393
mrpt::hwdrivers::CServoeNeck::setAngleWithFilter
bool setAngleWithFilter(double angle, const uint8_t servo=0, bool fast=false)
Turns the servo up to the specified angle (in radians in the range -pi,pi) filtered by average with t...
Definition: CServoeNeck.cpp:308
mrpt::hwdrivers
Contains classes for various device interfaces.
Definition: C2DRangeFinderAbstract.h:22
mrpt::comms
Serial and networking devices and utilities.
Definition: CClientTCPSocket.h:21
mrpt::serialization::CMessage::type
uint32_t type
An identifier of the message type (only the least-sig byte is typically sent)
Definition: CMessage.h:34
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
mrpt::hwdrivers::CServoeNeck::setAngle
bool setAngle(double angle, const uint8_t servo=0, bool fast=false)
Turns the servo up to the specified angle (in radians in the range -pi,pi, other values will be satur...
Definition: CServoeNeck.cpp:255
uint8_t
unsigned char uint8_t
Definition: rptypes.h:41
mrpt::RAD2DEG
double RAD2DEG(const double x)
Radians to degrees.
Definition: core/include/mrpt/core/bits_math.h:48
mrpt::hwdrivers::CServoeNeck::m_offsets
std::vector< float > m_offsets
The offset used for each servo.
Definition: CServoeNeck.h:133
M_PI
#define M_PI
Definition: core/include/mrpt/core/bits_math.h:38
mrpt::serialization::CMessage
A class that contain generic messages, that can be sent and received from a "CClientTCPSocket" object...
Definition: CMessage.h:29
mrpt::hwdrivers::CServoeNeck::disableServo
bool disableServo(const uint8_t servo=0)
Disables the servo so the neck will be loose.
Definition: CServoeNeck.cpp:331
CServoeNeck.h
mrpt::serialization::CMessage::content
std::vector< uint8_t > content
The contents of the message (memory is automatically handled by the std::vector object)
Definition: CMessage.h:37
mrpt::hwdrivers::CServoeNeck::getCurrentAngle
bool getCurrentAngle(double &angle, const uint8_t servo=0)
Gets the current angle of the servo (in radians within (-pi,pi))
Definition: CServoeNeck.cpp:240
mrpt::hwdrivers::CServoeNeck::setOffsets
void setOffsets(float offset0, float offset1, float offset2)
Load the Offset values for each servo.
Definition: CServoeNeck.cpp:427
mrpt::hwdrivers::CServoeNeck::m_usbSerialNumber
std::string m_usbSerialNumber
A copy of the device serial number (to open the USB FTDI chip).
Definition: CServoeNeck.h:121
mrpt::comms::CInterfaceFTDI::Close
void Close()
Close the USB device.
Definition: CInterfaceFTDI_WIN.cpp:285
mrpt::hwdrivers::CServoeNeck::getRegisterValue
bool getRegisterValue(uint16_t &value, const uint8_t servo=0)
Definition: CServoeNeck.cpp:201
mrpt::hwdrivers::CServoeNeck::m_TruncateFactor
double m_TruncateFactor
The range of turn of the servo will be truncated to "+-m_truncate_factor*(pi/2)".
Definition: CServoeNeck.h:126
mrpt::serialization
Definition: aligned_serialization.h:14
mrpt::hwdrivers::CServoeNeck::enableServo
bool enableServo(const uint8_t servo=0)
Enables the servo so the neck will be tight.
Definition: CServoeNeck.cpp:362
mrpt::serialization::CMessage::getContentAsString
void getContentAsString(std::string &str)
Gets the contents of the message as a string.
Definition: CMessage.cpp:96
mrpt::hwdrivers::CServoeNeck::~CServoeNeck
~CServoeNeck()
Definition: CServoeNeck.cpp:42
bits_math.h
mrpt::serialization::archiveFrom
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT's CStream,...
Definition: CArchive.h:561
net_utils.h
value
GLsizei const GLfloat * value
Definition: glext.h:4117
mrpt::hwdrivers::CServoeNeck::m_PrevAngles
std::deque< double > m_PrevAngles
A vector containing the last N angles which where passed to the servo (for averaging)
Definition: CServoeNeck.h:129
mrpt::hwdrivers::CServoeNeck::setAngleAndSpeed
bool setAngleAndSpeed(double angle, const uint8_t servo, const uint8_t speed)
Turns the servo up to the specified angle (in radians in the range -pi,pi, other values will be satur...
Definition: CServoeNeck.cpp:285
mrpt::hwdrivers::CServoeNeck::setRegisterValue
bool setRegisterValue(const uint16_t value, const uint8_t servo=0, bool fast=false)
Definition: CServoeNeck.cpp:125
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::comms::CInterfaceFTDI::OpenBySerialNumber
void OpenBySerialNumber(const std::string &serialNumber)
Open by device serial number.
Definition: CInterfaceFTDI_WIN.cpp:425
iterator
Scalar * iterator
Definition: eigen_plugins.h:26
mrpt::hwdrivers::CServoeNeck::regValue2angle
double regValue2angle(const uint16_t value)
Converts from a certain value of the ATMEGA16 PWM register to the corresponding decimal angle (for in...
Definition: CServoeNeck.cpp:99
CArchive.h
mrpt::comms::CInterfaceFTDI::SetLatencyTimer
void SetLatencyTimer(unsigned char latency_ms)
Change the latency timer (in milliseconds) implemented on the FTDI chip: for a few ms,...
Definition: CInterfaceFTDI_WIN.cpp:371
mrpt::comms::CInterfaceFTDI::SetTimeouts
void SetTimeouts(unsigned long dwReadTimeout_ms, unsigned long dwWriteTimeout_ms)
Change read & write timeouts, in milliseconds.
Definition: CInterfaceFTDI_WIN.cpp:349
CMessage.h
hwdrivers-precomp.h


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST