MRPT  2.0.4
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>
14 #include <mrpt/hwdrivers/CServoeNeck.h>
15 #include <mrpt/serialization/CArchive.h>
16 #include <mrpt/serialization/CMessage.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  {
49  mrpt::serialization::CMessage msg, msgRx;
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 -------------------------------------------------------------*/
124 bool CServoeNeck::setRegisterValue(
125  const uint16_t value, const uint8_t servo, bool fast)
126 {
127  try
128  {
129  if (!isOpen()) return false;
130 
131  mrpt::serialization::CMessage msg, msgRx;
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 -------------------------------------------------------------*/
162 bool CServoeNeck::setRegisterValueAndSpeed(
163  const uint16_t value, const uint8_t servo, const uint16_t speed)
164 {
165  try
166  {
167  if (!isOpen()) return false;
168 
169  mrpt::serialization::CMessage msg, msgRx;
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 
206  mrpt::serialization::CMessage msg, msgRx;
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 -------------------------------------------------------------*/
284 bool CServoeNeck::setAngleAndSpeed(
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 -------------------------------------------------------------*/
307 bool CServoeNeck::setAngleWithFilter(
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 
336  mrpt::serialization::CMessage msg, msgRx;
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 
367  mrpt::serialization::CMessage msg, msgRx;
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 -------------------------------------------------------------*/
401 bool CServoeNeck::checkConnectionAndConnect()
402 {
403  if (isOpen()) return true;
404 
405  try
406  {
407  OpenBySerialNumber(m_usbSerialNumber);
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 }
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:76
mrpt::comms::CInterfaceFTDI::isOpen
bool isOpen()
Checks whether the chip has been successfully open.
Definition: CInterfaceFTDI_WIN.cpp:181
mrpt::hwdrivers::CServoeNeck::setRegisterValueAndSpeed
bool setRegisterValueAndSpeed(const uint16_t value, const uint8_t servo, const uint16_t speed)
Definition: CServoeNeck.cpp:162
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:254
mrpt::hwdrivers::CServoeNeck::checkConnectionAndConnect
bool checkConnectionAndConnect()
Tries to connect to the USB device (if disconnected).
Definition: CServoeNeck.cpp:401
mrpt::hwdrivers::CServoeNeck::queryFirmwareVersion
bool queryFirmwareVersion(std::string &out_firmwareVersion)
Gets the firmware version of the eNeck board.
Definition: CServoeNeck.cpp:45
mrpt::serialization::CMessage::type
uint32_t type
An identifier of the message type (only the least-sig byte is typically sent)
Definition: CMessage.h:32
mrpt::hwdrivers
Contains classes for various device interfaces.
Definition: C2DRangeFinderAbstract.h:19
mrpt::hwdrivers::CServoeNeck::m_NumPrevAngles
unsigned int m_NumPrevAngles
Number of previous angles to store for averaging.
Definition: CServoeNeck.h:128
mrpt::hwdrivers::CServoeNeck::center
bool center(const uint8_t servo=0)
Centers the servo at zero position.
Definition: CServoeNeck.cpp:392
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:307
mrpt::hwdrivers::CServoeNeck::m_offsets
std::vector< float > m_offsets
The offset used for each servo.
Definition: CServoeNeck.h:130
mrpt::serialization::archiveFrom
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
CMessage.h
bits_math.h
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:239
M_PI
#define M_PI
Definition: core/include/mrpt/core/bits_math.h:43
CArchive.h
mrpt::comms::CInterfaceFTDI::Close
void Close()
Close the USB device.
Definition: CInterfaceFTDI_WIN.cpp:287
mrpt::hwdrivers::CServoeNeck::disableServo
bool disableServo(const uint8_t servo=0)
Disables the servo so the neck will be loose.
Definition: CServoeNeck.cpp:330
mrpt::comms::CInterfaceFTDI::SetTimeouts
void SetTimeouts(unsigned long dwReadTimeout_ms, unsigned long dwWriteTimeout_ms)
Change read & write timeouts, in milliseconds.
Definition: CInterfaceFTDI_WIN.cpp:351
mrpt::hwdrivers::CServoeNeck::enableServo
bool enableServo(const uint8_t servo=0)
Enables the servo so the neck will be tight.
Definition: CServoeNeck.cpp:361
mrpt::comms::CInterfaceFTDI::OpenBySerialNumber
void OpenBySerialNumber(const std::string &serialNumber)
Open by device serial number.
Definition: CInterfaceFTDI_WIN.cpp:427
mrpt::hwdrivers::CServoeNeck::setOffsets
void setOffsets(float offset0, float offset1, float offset2)
Load the Offset values for each servo.
Definition: CServoeNeck.cpp:426
CServoeNeck.h
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::serialization::CMessage
A class that contain generic messages, that can be sent and received from a "CClientTCPSocket" object...
Definition: CMessage.h:27
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::m_usbSerialNumber
std::string m_usbSerialNumber
A copy of the device serial number (to open the USB FTDI chip).
Definition: CServoeNeck.h:118
mrpt::RAD2DEG
constexpr double RAD2DEG(const double x)
Radians to degrees.
Definition: core/include/mrpt/core/bits_math.h:56
mrpt::hwdrivers::CServoeNeck::getRegisterValue
bool getRegisterValue(uint16_t &value, const uint8_t servo=0)
Definition: CServoeNeck.cpp:200
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:123
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:35
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:284
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:126
mrpt::comms
Serial and networking devices and utilities.
Definition: CClientTCPSocket.h:21
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:98
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:373
mrpt::hwdrivers::CServoeNeck::setRegisterValue
bool setRegisterValue(const uint16_t value, const uint8_t servo=0, bool fast=false)
Definition: CServoeNeck.cpp:124
mrpt::comms::CInterfaceFTDI::Purge
void Purge()
Purge the I/O buffers.
Definition: CInterfaceFTDI_WIN.cpp:339


Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020