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



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018