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



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020