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


Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019