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


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019