28 bool CPtuDPerception::moveToAbsPos(
char axis,
double nRad) {
30 if (!radAsign(axis,
'P',nRad))
return false;
40 bool CPtuDPerception::absPosQ(
char axis,
double &nRad) {
42 return radQuerry(axis,
'P',nRad);
50 bool CPtuDPerception::moveToOffPos(
char axis,
double nRad) {
52 if (!radAsign(axis,
'O',nRad))
return false;
62 bool CPtuDPerception::offPosQ(
char axis,
double &nRad) {
64 return radQuerry(axis,
'O',nRad);
72 bool CPtuDPerception::maxPosQ(
char axis,
double &nRad) {
74 return radQuerry(axis,
'X',nRad);
82 bool CPtuDPerception::minPosQ(
char axis,
double &nRad) {
84 return radQuerry(axis,
'N',nRad);
92 bool CPtuDPerception::speed(
char axis,
double radSec) {
94 return radAsign(axis,
'S',radSec);
101 bool CPtuDPerception::speedQ(
char axis,
double &radSec) {
103 return radQuerry(axis,
'S',radSec);
111 bool CPtuDPerception::aceleration(
char axis,
double radSec2){
113 return radAsign(axis,
'A',radSec2);
121 bool CPtuDPerception::acelerationQ(
char axis,
double &radSec2) {
123 return radQuerry(axis,
'A',radSec2);
131 bool CPtuDPerception::baseSpeed(
char axis,
double radSec) {
133 return radAsign(axis,
'B',radSec);
140 bool CPtuDPerception::baseSpeedQ(
char axis,
double &radSec) {
142 return radQuerry(axis,
'B',radSec);
150 bool CPtuDPerception::upperSpeed(
char axis,
double radSec) {
152 return radAsign(axis,
'U',radSec);
159 bool CPtuDPerception::upperSpeedQ(
char axis,
double &radSec) {
161 return radQuerry(axis,
'U',radSec);
169 bool CPtuDPerception::lowerSpeed(
char axis,
double radSec) {
171 return radAsign(axis,
'L',radSec);
179 bool CPtuDPerception::lowerSpeedQ(
char axis,
double &radSec) {
181 return radQuerry(axis,
'L',radSec);
189 bool CPtuDPerception::enableLimitsQ(
bool &enable) {
193 if (!transmit(
"L") || !receive(
"L",response))
return false;
195 if (strstr(
upperCase(response).c_str(),
"ENABLE")!=NULL) enable=
true;
206 bool CPtuDPerception::enableLimits(
bool set) {
208 if (
set)
return ( transmit(
"LE") && receive(
"LE",NULL) );
209 else return (transmit(
"LD") && receive(
"LD",NULL) );
217 bool CPtuDPerception::inmediateExecution(
bool set) {
219 if (
set)
return ( transmit(
"I") && receive(
"I",NULL) );
220 else return ( transmit(
"S") && receive(
"S",NULL) );
229 bool CPtuDPerception::aWait(
void) {
231 return ( transmit(
"A") && receive(
"A",NULL) );
239 bool CPtuDPerception::haltAll() {
241 return ( transmit(
"H") && receive(
"H",NULL) );
248 bool CPtuDPerception::halt(
char axis) {
251 sTrans[0]=
'H';sTrans[1]=axis;sTrans[2]=
'\0';
253 return ( transmit(sTrans) && receive(sTrans,NULL) );
263 if (!transmit(
"R"))
return false;
266 return panTiltHitError();
273 bool CPtuDPerception::save(
void) {
275 return ( transmit(
"DS") && receive(
"DS",NULL) );
283 bool CPtuDPerception::restoreDefaults(
void){
285 return ( transmit(
"DR") && receive(
"DR",NULL) );
293 bool CPtuDPerception::restoreFactoryDefaults(
void){
295 return ( transmit(
"DF") && receive(
"DF",NULL) );
305 return ( transmit(
"V") && receive(
"V",sVersion) );
314 bool CPtuDPerception::powerModeQ(
bool transit,
char &
mode){
316 const char * response=
"";
320 if (!transmit(
"PM"))
return false;
322 if (!transmit(
"PH"))
return false;
325 if (strstr(
upperCase(response).c_str(),
"REGULAR")!=NULL)
327 else if (strstr(
upperCase(response).c_str(),
"HIGH")!=NULL)
329 else if (strstr(
upperCase(response).c_str(),
"LOW")!=NULL)
342 bool CPtuDPerception::powerMode(
bool transit,
char mode){
346 sTrans[1]= transit ?
'M':
'H';
350 return ( transmit(sTrans) && receive(sTrans,NULL) );
358 bool CPtuDPerception::init(
const string &port){
364 cout << endl <<
"[INFO] Start PTU comunication config:" << endl;
366 cout <<
"[PTU::OpenSerialPort] Opening serial port...";
368 if(serPort.isOpen()) {
371 cout <<
" Error opening serial port";
375 cout <<
"OK" << endl;
377 cout <<
"[PTU::SetTimeouts] Setting timeouts...";
378 serPort.setTimeouts(1000,1,1000, 1, 1000);
379 cout <<
"OK" << endl;
381 cout <<
"[PTU::setBaudRate] Setting baud rate...";
382 serPort.setConfig(9600);
383 cout <<
"OK" << endl;
386 cout <<
"[PTU::setInitialConfiguration] Setting initial configuration...";
387 if ( (!verbose(
true)) ||
390 (!inmediateExecution(
true))
392 cout <<
" Error setting initial configuration";
397 cout <<
"OK" << endl << endl <<
"[INFO] Pan Resolution: " << panResolution <<
" radians | " <<
RAD2DEG(panResolution) <<
"degrees";
398 cout << endl <<
"[INFO] TitlResolution: " << tiltResolution <<
" radians | " <<
RAD2DEG(tiltResolution) <<
"degrees" << endl << endl;
403 cerr << e.what() << endl;
416 void CPtuDPerception::close(){
426 double CPtuDPerception::radError(
char axis,
double nRadMoved) {
431 div=nRadMoved-long(nRadMoved/panResolution)*panResolution;
433 div=nRadMoved-long(nRadMoved/tiltResolution)*tiltResolution;
443 bool CPtuDPerception::transmit(
const char * command) {
450 size_t written = serPort.Write(str,strlen(str));
464 bool CPtuDPerception::receive(
const char * command,
char * response) {
467 unsigned long nReaden;
472 nReaden=serPort.Read(&str[cnt],1);
473 if (nReaden!=0) cnt++;
474 }
while ( (nReaden!=0) && (((tmp=strstr(str,command))==NULL) ||
475 (str[cnt-1]!=
'\n')) );
477 if (nReaden==0) { nError=nError*TimeoutError;
return false; }
480 if (response!=NULL) {
487 if (strstr(tmp,
"!")==NULL) { nError=nError*NoError;
return true; }
489 if ((strstr(tmp,
"!P")!=NULL) && (strstr(tmp,
"!T")!=NULL )) nError=nError*PanTiltHitError;
490 else if (strstr(tmp,
"!T")!=NULL ) nError=nError*TiltHitError;
491 else if (strstr(tmp,
"!P")!=NULL) nError=nError*PanHitError;
492 else if (strstr(tmp,
"! Maximum")!=NULL) nError=nError*MaxLimitError;
493 else if (strstr(tmp,
"! Minimum")!=NULL) nError=nError*MinLimitError;
494 else if (strstr(tmp,
"! Value")!=NULL) nError=nError*OutOfRange;
495 else if (strstr(tmp,
"! Illegal")!=NULL) nError=nError*IllegalCommandError;
496 else nError=nError*UnExpectedError;
507 bool CPtuDPerception::verboseQ(
bool &
mode) {
511 if (!transmit(
"F") || !receive(
"F",response))
return false;
513 if (strstr(response,
"VERBOSE")!=NULL)
mode=
true;
525 bool CPtuDPerception::verbose(
bool set) {
527 if (
set)
return (transmit(
"FV") && (receive(
"FV",NULL)) );
528 else return (transmit(
"FT") && (receive(
"FT",NULL)) );
536 bool CPtuDPerception::echoModeQ(
bool &
mode) {
540 if (!transmit(
"E") || !receive(
"E",response))
return false;
542 if (strstr(
upperCase(response).c_str(),
"ENABLE")!=NULL)
mode=
true;
553 bool CPtuDPerception::echoMode(
bool mode) {
555 if (
mode)
return (transmit(
"EE") && receive(
"EE",NULL) );
556 else return (transmit(
"ED") && receive(
"ED",NULL) );
564 bool CPtuDPerception::resolution(
void) {
568 if ( (!transmit(
"PR")) || (!receive(
"PR",response)) )
return false;
569 panResolution=
DEG2RAD(convertToDouble(response) / 3600);
571 if ( (!transmit(
"TR")) || (!receive(
"TR",response)) )
return false;
572 tiltResolution=
DEG2RAD(convertToDouble(response) / 3600);
582 bool CPtuDPerception::radQuerry(
char axis,
char command,
double &rad) {
592 if ( ( !transmit(sTrans) ) || (!receive(sTrans,response)) )
return false;
604 bool CPtuDPerception::radAsign(
char axis,
char command,
double nRad) {
611 os::sprintf(sPos,
sizeof(sPos),
"%li", radToPos(axis,nRad));
617 return (transmit(sTrans) && receive(sTrans,response));
625 bool CPtuDPerception::scan(
char axis,
int tWait,
float initial,
float final,
double radPre){
635 moveToAbsPos(axis,initial);
643 long steps = radToPos(axis,radPre);
647 int initialPos = radToPos(axis,initial);
648 int finalPos = radToPos(axis,
final);
650 totalSteps = abs(initialPos-finalPos);
653 for(
int i=0;i<totalSteps/steps;i++)
656 moveToOffPos(axis,-radPre);
658 moveToOffPos(axis,radPre);
665 moveToOffPos(axis,radPre/2);
669 for(
int i=0;i<(totalSteps/steps)-1;i++)
672 moveToOffPos(axis,radPre);
674 moveToOffPos(axis,-radPre);
683 moveToAbsPos(axis,0);
693 long CPtuDPerception::radToPos(
char axis,
double nrad) {
695 if (axis==Pan)
return (
long) (nrad / panResolution);
696 else return (
long) (nrad / tiltResolution);
704 double CPtuDPerception::posToRad(
char axis,
long nPos) {
706 if (axis==Pan)
return (
double) nPos * panResolution;
707 else return (
double) nPos * tiltResolution;
717 char * result=strpbrk(sLong,
"-0123456789");
720 return strtol(result,&stop,10);
728 double CPtuDPerception::convertToDouble(
char *sDouble) {
730 char * result=strpbrk(sDouble,
"-0123456789");
733 return strtod(result,&stop);
741 int CPtuDPerception::checkErrors(){
751 }
else if(timeoutError()){
753 }
else if(initError()){
755 }
else if(panTiltHitError()){
757 }
else if(panHitError()){
759 }
else if(tiltHitError()){
761 }
else if(maxLimitError()){
763 }
else if(minLimitError()){
765 }
else if(outOfRange()){
767 }
else if(illegalCommandError()){
769 }
else if(unExpectedError()){
781 void CPtuDPerception::nversion(
double &nVersion) {
782 cout <<
"[ERROR] Function not defined for this PTU model";
791 bool CPtuDPerception::setLimits(
char axis,
double &l,
double &u) {
793 cout <<
"[ERROR] Function not defined for this PTU model";
802 bool CPtuDPerception::changeMotionDir() {
803 cout <<
"[ERROR] Function not defined for this PTU model";
812 bool CPtuDPerception::rangeMeasure() {
813 cout <<
"[ERROR] Function not defined for this PTU model";
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Contains classes for various device interfaces.
char BASE_IMPEXP * strcpy(char *dest, size_t destSize, const char *source) MRPT_NO_THROWS
An OS-independent version of strcpy.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
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...
long convertToLong(char *sLong)
std::string BASE_IMPEXP upperCase(const std::string &str)
Returns a upper-case version of a string.
char BASE_IMPEXP * strcat(char *dest, size_t destSize, const char *source) MRPT_NO_THROWS
An OS-independent version of strcat.
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...