Main MRPT website > C++ reference for MRPT 1.9.9
CTuMicos.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/hwdrivers/CTuMicos.h>
13 #include <mrpt/system/string_utils.h>
14 #include <mrpt/utils/utils_defs.h>
15 
16 #include <cstring>
17 #include <cstdio>
18 #include <thread>
19 
20 using namespace std;
21 using namespace mrpt::utils;
22 using namespace mrpt::hwdrivers;
23 using namespace mrpt::system;
24 
25 /*-------------------------------------------------------------
26  rangeMeasure
27 -------------------------------------------------------------*/
28 
29 bool CTuMicos::rangeMeasure()
30 {
31  char command[50];
32 
33  // char command2[50];
34  // sprintf(command2,"%u %s ",axis_index,"nreset");
35 
36  // if (!transmit(command2)) return false;
37 
38  sprintf(command, "%u %s ", axis_index, "nrm");
39 
40  if (!transmit(command)) return false;
41 
42  return true;
43 }
44 
45 /*-------------------------------------------------------------
46  moveToAbsPos
47 -------------------------------------------------------------*/
48 
49 bool CTuMicos::moveToAbsPos(char axis, double nRad)
50 {
51  char command[300];
52  sprintf(command, "%f %u %s", RAD2DEG(nRad), axis_index, "nm");
53 
54  if (!transmit(command)) return false;
55 
56  return true;
57 }
58 
59 /*-------------------------------------------------------------
60  absPosQ
61 -------------------------------------------------------------*/
62 
63 bool CTuMicos::absPosQ(char axis, double& nRad)
64 {
65  return radQuerry(axis, 'p', nRad);
66 }
67 
68 /*-------------------------------------------------------------
69  moveToOffPos
70 -------------------------------------------------------------*/
71 
72 bool CTuMicos::moveToOffPos(char axis, double nRad)
73 {
74  char command[300];
75  sprintf(command, "%f %u %s", RAD2DEG(nRad), axis_index, "nr");
76 
77  if (!transmit(command)) return false;
78 
79  return true;
80 }
81 
82 /*-------------------------------------------------------------
83  offPosQ
84 -------------------------------------------------------------*/
85 
86 bool CTuMicos::offPosQ(char axis, double& nRad)
87 {
88  return radQuerry(axis, 'p', nRad);
89 }
90 
91 /*-------------------------------------------------------------
92  maxPosQ
93 -------------------------------------------------------------*/
94 
95 bool CTuMicos::maxPosQ(char axis, double& nRad)
96 {
97  return radQuerry('u', 'l', nRad); // Up limit
98 }
99 
100 /*-------------------------------------------------------------
101  minPosQ
102 -------------------------------------------------------------*/
103 
104 bool CTuMicos::minPosQ(char axis, double& nRad)
105 {
106  return radQuerry('l', 'l', nRad); // Low limit
107 }
108 
109 /*-------------------------------------------------------------
110  speed
111 -------------------------------------------------------------*/
112 
113 bool CTuMicos::speed(char axis, double radSec)
114 {
115  return radAsign(axis, 'v', radSec);
116 }
117 
118 /*-------------------------------------------------------------
119  speedQ
120 -------------------------------------------------------------*/
121 
122 bool CTuMicos::speedQ(char axis, double& radSec)
123 {
124  return radQuerry(axis, 'v', radSec);
125 }
126 
127 /*-------------------------------------------------------------
128  aceleration
129 -------------------------------------------------------------*/
130 
131 bool CTuMicos::aceleration(char axis, double radSec2)
132 {
133  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
134 
135  return false;
136 }
137 
138 /*-------------------------------------------------------------
139  acelerationQ
140 -------------------------------------------------------------*/
141 
142 bool CTuMicos::acelerationQ(char axis, double& radSec2)
143 {
144  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
145 
146  return false;
147 }
148 
149 /*-------------------------------------------------------------
150  baseSpeed
151 -------------------------------------------------------------*/
152 
153 bool CTuMicos::baseSpeed(char axis, double radSec)
154 {
155  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
156 
157  return false;
158 }
159 
160 /*-------------------------------------------------------------
161  baseSpeedQ
162 -------------------------------------------------------------*/
163 
164 bool CTuMicos::baseSpeedQ(char axis, double& radSec)
165 {
166  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
167 
168  return false;
169 }
170 
171 /*-------------------------------------------------------------
172  upperSpeed
173 -------------------------------------------------------------*/
174 
175 bool CTuMicos::upperSpeed(char axis, double radSec)
176 {
177  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
178 
179  return false;
180 }
181 
182 /*-------------------------------------------------------------
183  upperSpeedQ
184 -------------------------------------------------------------*/
185 
186 bool CTuMicos::upperSpeedQ(char axis, double& radSec)
187 {
188  radSec = DEG2RAD(26);
189 
190  return true;
191 }
192 
193 /*-------------------------------------------------------------
194  lowerSpeed
195 -------------------------------------------------------------*/
196 
197 bool CTuMicos::lowerSpeed(char axis, double radSec)
198 {
199  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
200 
201  return false;
202 }
203 
204 /*-------------------------------------------------------------
205  lowerSpeedQ
206 -------------------------------------------------------------*/
207 
208 bool CTuMicos::lowerSpeedQ(char axis, double& radSec)
209 {
210  radSec = DEG2RAD(1);
211 
212  return true;
213 }
214 
215 /*-------------------------------------------------------------
216  enableLimitsQ
217 -------------------------------------------------------------*/
218 
219 bool CTuMicos::enableLimitsQ(bool& enable)
220 {
221  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
222 
223  return false;
224 }
225 
226 /*-------------------------------------------------------------
227  enableLimits
228 -------------------------------------------------------------*/
229 
230 bool CTuMicos::enableLimits(bool set)
231 {
232  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
233 
234  return false;
235 }
236 
237 /*-------------------------------------------------------------
238  inmediateExecution
239 -------------------------------------------------------------*/
240 
241 bool CTuMicos::inmediateExecution(bool set)
242 {
243  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
244 
245  return false;
246 }
247 
248 /*-------------------------------------------------------------
249  aWait
250 -------------------------------------------------------------*/
251 
252 bool CTuMicos::aWait(void) { return true; }
253 /*-------------------------------------------------------------
254  haltAll
255 -------------------------------------------------------------*/
256 
257 bool CTuMicos::haltAll()
258 {
259  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
260 
261  return false;
262 }
263 
264 /*-------------------------------------------------------------
265  halt
266 -------------------------------------------------------------*/
267 
268 bool CTuMicos::halt(char axis)
269 {
270  char command[50];
271  sprintf(command, "%u %s", axis_index, "nabort");
272 
273  if (!transmit(command)) return false;
274 
275  return true;
276 }
277 
278 /*-------------------------------------------------------------
279  reset
280 -------------------------------------------------------------*/
281 
282 bool CTuMicos::reset(void)
283 {
284  char command[50], command2[50];
285  sprintf(command, "%u %s ", axis_index, "nreset");
286 
287  if (!transmit(command)) return false;
288 
289  sprintf(command2, "%u %s ", axis_index, "ncal");
290 
291  std::this_thread::sleep_for(1000ms);
292 
293  if (!transmit(command2)) return false;
294 
295  return true;
296 }
297 
298 /*-------------------------------------------------------------
299  save
300 -------------------------------------------------------------*/
301 
302 bool CTuMicos::save(void)
303 {
304  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
305 
306  return false;
307 }
308 
309 /*-------------------------------------------------------------
310  restoreDefaults
311 -------------------------------------------------------------*/
312 
313 bool CTuMicos::restoreDefaults(void)
314 {
315  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
316 
317  return false;
318 }
319 
320 /*-------------------------------------------------------------
321  restoreFactoryDefaults
322 -------------------------------------------------------------*/
323 
324 bool CTuMicos::restoreFactoryDefaults(void)
325 {
326  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
327 
328  return false;
329 }
330 
331 /*-------------------------------------------------------------
332  version
333 -------------------------------------------------------------*/
334 
335 bool CTuMicos::version(char* sVersion)
336 {
337  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
338 
339  return false;
340 }
341 
342 /*-------------------------------------------------------------
343  nversion
344 -------------------------------------------------------------*/
345 
346 void CTuMicos::nversion(double& nVersion)
347 {
348  if (!radQuerry(0, 'n', nVersion))
349  throw std::runtime_error("INCORRECT VERSION");
350 }
351 
352 /*-------------------------------------------------------------
353  powerModeQ
354 -------------------------------------------------------------*/
355 
356 bool CTuMicos::powerModeQ(bool transit, char& mode)
357 {
358  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
359 
360  return false;
361 }
362 
363 /*-------------------------------------------------------------
364  powerMode
365 -------------------------------------------------------------*/
366 
367 bool CTuMicos::powerMode(bool transit, char mode)
368 {
369  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
370 
371  return false;
372 }
373 
374 /*-------------------------------------------------------------
375  clear
376 -------------------------------------------------------------*/
377 
378 bool CTuMicos::clear()
379 {
380  char command[300];
381  sprintf(command, "%u %s", axis_index, "nclear");
382 
383  if (!transmit(command)) return false;
384 
385  return true;
386 }
387 
388 /*-------------------------------------------------------------
389  setLimits
390 -------------------------------------------------------------*/
391 
392 bool CTuMicos::setLimits(char axis, double& l, double& u)
393 {
394  char command[300] = "";
395  sprintf(command, "%f %f %u setnlimit", l, u, axis_index);
396 
397  if (!transmit(command)) return false;
398 
399  return true;
400 }
401 
402 /*-------------------------------------------------------------
403  changeMotionDir
404 -------------------------------------------------------------*/
405 
406 bool CTuMicos::changeMotionDir()
407 {
408  double motionDir;
409  unsigned int newMotionDir;
410  char command[300] = "";
411 
412  // Otains actual motion dir
413  if (!radQuerry(0, 'c', motionDir)) return false;
414 
415  if (!motionDir)
416  newMotionDir = 1;
417  else
418  newMotionDir = 0;
419 
420  // Change motion direction
421  sprintf(command, "%u %u setmotiondir", newMotionDir, axis_index);
422 
423  if (!transmit(command)) return false;
424 
425  return true;
426 }
427 
428 /*-------------------------------------------------------------
429  init
430 -------------------------------------------------------------*/
431 
432 bool CTuMicos::init(const string& port)
433 {
434  try
435  {
436  serPort.open(port);
437 
438  cout << endl << "[INFO] Start Tu MICOS comunication config:" << endl;
439 
440  cout << "[PTU::OpenSerialPort] Opening serial port...";
441 
442  if (serPort.isOpen())
443  {
444  cout << "OK" << endl;
445  }
446  else
447  {
448  cout << " Error opening serial port";
449  return false;
450  }
451 
452  cout << "[PTU::SetTimeouts] Setting timeouts...";
453  serPort.setTimeouts(1000, 1, 1000, 1, 1000);
454  cout << "OK" << endl;
455 
456  cout << "[PTU::setBaudRate] Setting baud rate...";
457  serPort.setConfig(19200);
458  cout << "OK" << endl;
459 
460  // PTU initial configuration
461  cout << "[PTU::setInitialConfiguration] Setting initial "
462  "configuration...";
463 
464  axis_index = 1;
465  double version;
466  nversion(version);
467  if ((!version) || (!clear()))
468  {
469  cout << " Error setting initial configuration";
470  serPort.close();
471  return false;
472  }
473 
474  cout << "OK" << endl;
475  }
476  catch (std::exception& e)
477  {
478  MRPT_LOG_ERROR_STREAM("Error initializating: " << e.what());
479  return false;
480  }
481  catch (...)
482  {
483  MRPT_LOG_ERROR_STREAM("Error initializating.");
484  return false;
485  }
486 
487  return true;
488 }
489 
490 /*-------------------------------------------------------------
491  close
492 -------------------------------------------------------------*/
493 
494 void CTuMicos::close()
495 {
496  // Check if serPort is open
497  if (serPort.isOpen())
498  {
499  serPort.close();
500  cout << endl << "[INFO] TuMICOS Serial port closed" << endl;
501  }
502 }
503 
504 /*-------------------------------------------------------------
505  radError
506 -------------------------------------------------------------*/
507 
508 double CTuMicos::radError(char axis, double nRadMoved)
509 {
510  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
511 
512  return false;
513 }
514 
515 /*-------------------------------------------------------------
516  transmit
517 -------------------------------------------------------------*/
518 
519 bool CTuMicos::transmit(const char* command)
520 {
521  char str[300] = "";
522 
523  // Copy command in str char
524  strcpy(str, command);
525  strcat(str, " ");
526 
527  // Wirte in serial port
528  size_t written = serPort.Write(str, strlen(str));
529 
530  if (!written)
531  {
532  return false;
533  }
534 
535  return true;
536 }
537 
538 /*-------------------------------------------------------------
539  receive
540 -------------------------------------------------------------*/
541 
542 bool CTuMicos::receive(const char* command, char* response)
543 {
544  int cnt = 0;
545  unsigned long nReaden;
546  char str[150]; //="";
547  // char * tmp="";
548 
549  do
550  {
551  nReaden = serPort.Read(&str[cnt], 1);
552  if (nReaden != 0) cnt++;
553  } while ((nReaden != 0) && (str[cnt - 1] != '\n'));
554 
555  if (nReaden == 0) return false;
556 
557  // cout << str << endl;
558 
559  if (str[0])
560  {
561  strcpy(response, str);
562  return true;
563  }
564 
565  return false;
566 }
567 
568 /*-------------------------------------------------------------
569  verboseQ
570 -------------------------------------------------------------*/
571 
572 bool CTuMicos::verboseQ(bool& mode)
573 {
574  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
575 
576  return false;
577 }
578 
579 /*-------------------------------------------------------------
580  verbose
581 -------------------------------------------------------------*/
582 
583 bool CTuMicos::verbose(bool set)
584 {
585  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
586 
587  return false;
588 }
589 
590 /*-------------------------------------------------------------
591  echoModeQ
592 -------------------------------------------------------------*/
593 
594 bool CTuMicos::echoModeQ(bool& mode)
595 {
596  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
597 
598  return false;
599 }
600 
601 /*-------------------------------------------------------------
602  echoMode
603 -------------------------------------------------------------*/
604 
605 bool CTuMicos::echoMode(bool mode)
606 {
607  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
608 
609  return false;
610 }
611 
612 /*-------------------------------------------------------------
613  resolution
614 -------------------------------------------------------------*/
615 
616 bool CTuMicos::resolution(void)
617 {
618  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
619 
620  return false;
621 }
622 
623 /*-------------------------------------------------------------
624  status
625 -------------------------------------------------------------*/
626 
627 double CTuMicos::status(double& rad) { return radQuerry(0, 's', rad); }
628 /*-------------------------------------------------------------
629  radQuerry
630 -------------------------------------------------------------*/
631 
632 bool CTuMicos::radQuerry(char axis, char command, double& rad)
633 {
634  char response[150];
635  char command2[300];
636  bool toRad = true, select = false;
637 
638  if (command == 'p')
639  { // Actual position
640  sprintf(command2, "%u %s", axis_index, "np");
641  }
642  else if (command == 'v')
643  { // Velocity for move
644  sprintf(command2, "%u %s", axis_index, "gnv");
645  }
646  else if (command == 's')
647  { // Actual status
648  sprintf(command2, "%u %s", axis_index, "nst");
649  toRad = false;
650  }
651  else if (command == 'e')
652  { // Errors
653  sprintf(command2, "%u %s", axis_index, "gne");
654  toRad = false;
655  }
656  else if (command == 'l')
657  { // Limits of the travel
658  sprintf(command2, "%u %s", axis_index, "getnlimit");
659  select = true;
660  }
661  else if (command == 'n')
662  { // Number of version
663  sprintf(command2, "%u %s", axis_index, "nversion");
664  toRad = false;
665  }
666  else if (command == 'c')
667  { // Motion direction
668  sprintf(command2, "%u %s", axis_index, "getmotiondir");
669  toRad = false;
670  }
671 
672  if ((!transmit(command2)) || (!receive(nullptr, response))) return false;
673 
674  // If we can convert the result to radians
675  if (toRad)
676  {
677  // If is necesary to select a part of the reponse
678  if (select)
679  {
680  char s2[] = " ";
681  char *ptr1, *ptr2;
682  char* strContext;
683  ptr1 = mrpt::system::strtok(response, s2, &strContext);
684  ptr2 = mrpt::system::strtok(nullptr, s2, &strContext);
685  if (axis == 'l')
686  rad = (long)atof((const char*)ptr1);
687  else
688  rad = (long)atof((const char*)ptr2);
689  }
690  else
691  {
692  // Else converts deegres to radians
693  rad = DEG2RAD((double)atof((const char*)response));
694  }
695  }
696  else
697  {
698  // Else converts char to long
699  rad = (long)atof((const char*)response);
700  }
701 
702  return true;
703 }
704 
705 /*-------------------------------------------------------------
706  radAsign
707 -------------------------------------------------------------*/
708 
709 bool CTuMicos::radAsign(char axis, char command, double nRad)
710 {
711  char command2[300];
712 
713  if (command == 'v')
714  {
715  sprintf(command2, "%f %u %s", RAD2DEG(nRad), axis_index, "snv");
716  }
717 
718  return transmit(command2);
719 }
720 
721 /*-------------------------------------------------------------
722  scan
723 -------------------------------------------------------------*/
724 
725 bool CTuMicos::scan(
726  char axis, int tWait, float initial, float final, double radPre)
727 {
728  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
729 
730  return false;
731 }
732 
733 /*-------------------------------------------------------------
734  radToPos
735 -------------------------------------------------------------*/
736 
737 long CTuMicos::radToPos(char axis, double nrad)
738 {
739  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
740 
741  return false;
742 }
743 
744 /*-------------------------------------------------------------
745  posToRad
746 -------------------------------------------------------------*/
747 
748 double CTuMicos::posToRad(char axis, long nPos)
749 {
750  cout << endl << "[ERROR] Command not defined for this PTunit" << endl;
751 
752  return false;
753 }
754 
755 /*-------------------------------------------------------------
756  convertToLong
757 -------------------------------------------------------------*/
758 
759 long CTuMicos::convertToLong(char* sLong)
760 {
761  long a = (long)atof((const char*)sLong);
762 
763  return a;
764 }
765 
766 /*-------------------------------------------------------------
767  convertToDouble
768 -------------------------------------------------------------*/
769 
770 double CTuMicos::convertToDouble(char* sDouble)
771 {
772  char* result = strpbrk(sDouble, "-0123456789");
773  char* stop;
774 
775  return strtod(result, &stop);
776 }
777 
778 /*-------------------------------------------------------------
779  checkError
780 -------------------------------------------------------------*/
781 
782 int CTuMicos::checkErrors()
783 {
784  double code = 0;
785 
786  radQuerry(0, 'e', code);
787 
788  if ((int)code == 0)
789  {
790  cout << endl << "[No Error]" << endl;
791  }
792  else
793  {
794  switch ((int)code)
795  {
796  case 1:
797  case 2:
798  case 3:
799  case 4:
800  cout << endl << "[Error] Internal error" << endl;
801  break;
802  case 1001:
803  cout << endl << "[Error] Wrong parameter type" << endl;
804  break;
805  case 1002:
806  cout << endl
807  << "[Error] Insufficient parameters on the stack" << endl;
808  break;
809  case 1003:
810  cout << endl << "[Error] Value range is exceeded" << endl;
811  break;
812  case 1004:
813  cout << endl
814  << "[Error] Movement range should be exceeded" << endl;
815  break;
816  case 1008:
817  cout << endl
818  << "[Error] Insufficient parameters on the stack" << endl;
819  break;
820  case 1015:
821  cout << endl
822  << "[Error] Parameter out of the movement area" << endl;
823  break;
824  case 2000:
825  cout << endl << "[Error] Unknown command" << endl;
826  break;
827  default:
828  break;
829  }
830  }
831 
832  return code;
833 }
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: CParticleFilter.h:223
mrpt::system
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:30
mrpt::hwdrivers
Contains classes for various device interfaces.
Definition: C2DRangeFinderAbstract.h:22
std
STL namespace.
mrpt::utils::clear
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:189
string_utils.h
utils_defs.h
mrpt::system::strtok
char * strtok(char *str, const char *strDelimit, char **context) noexcept
An OS-independent method for tokenizing a string.
Definition: string_utils.cpp:196
DEG2RAD
#define DEG2RAD
Definition: CColouredPointsMap.h:112
convertToLong
long convertToLong(char *sLong)
Definition: CRovio.cpp:400
mode
GLint mode
Definition: glext.h:5669
RAD2DEG
#define RAD2DEG
Definition: CColouredPointsMap.h:115
code
Definition: inftrees.h:28
mrpt::system::os::strcat
char * strcat(char *dest, size_t destSize, const char *source) noexcept
An OS-independent version of strcat.
Definition: os.cpp:281
reset
GLboolean reset
Definition: glext.h:3582
status
_u8 status
Definition: rplidar_cmd.h:20
mrpt::system::os::strcpy
char * strcpy(char *dest, size_t destSize, const char *source) noexcept
An OS-independent version of strcpy.
Definition: os.cpp:296
MRPT_LOG_ERROR_STREAM
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
a
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
mrpt::system::os::sprintf
int sprintf(char *buf, size_t bufSize, const char *format,...) noexcept MRPT_printf_format_check(3
An OS-independent version of sprintf (Notice the bufSize param, which may be ignored in some compiler...
Definition: os.cpp:188
CTuMicos.h


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