hokuyo.cpp

00001 // J Bradshaw 20160304
00002 
00003 #include "hokuyo.h"
00004 
00005 Hokuyo::Hokuyo(PinName tx, PinName rx) : _hokuyo(tx, rx) {
00006     _hokuyo.baud(19200);
00007 }
00008 
00009 int Hokuyo::lidar_read(char *str, int numchars, float timeoutDelay){        
00010     Timer t_Frame;
00011     int i=0;        
00012     int timeoutState=0;
00013     float timeout;
00014     
00015     t_Frame.start();
00016     
00017     while(timeoutState != 3  && (i < numchars)){
00018         switch(timeoutState){
00019             case 0:
00020                 while(_hokuyo.readable()){ //if characters are in buffer, read them
00021                     str[i++] = _hokuyo.getc();
00022                     if(i == numchars){
00023                         timeoutState = 3;
00024                         break;   
00025                     }
00026                 }
00027                 //if no characters in buffer, initiate timeout
00028                 timeoutState = 1;
00029                 break;
00030             case 1:
00031                 timeout = t_Frame.read() + timeoutDelay;    //current time plus timeout time
00032                 timeoutState = 2;
00033 //                                              pc.printf("Timeout initiated %f\r\n", timeout);
00034                 break;
00035             case 2:
00036                 if(_hokuyo.readable()){ //check buffer while timeout is running
00037                     str[i++] = _hokuyo.getc();
00038                     if(i == numchars){
00039                         timeoutState = 3;
00040                     }
00041                     else{
00042                         timeoutState = 0;
00043                     }
00044                     break;
00045                 }
00046                 if(t_Frame.read() >= timeout) //if timeout has elapsed, exit the while loop with state 3
00047                     timeoutState = 3;
00048                 break;
00049             default:
00050                 timeoutState = 0;
00051         }//switch timeoutState                             
00052     }//while timeoutState != 2
00053     return i;   //return number of bytes read
00054 }
00055 
00056 int Hokuyo::Init(void)
00057 {
00058     char sent[20];
00059     
00060     _hokuyo.printf("L1\n");        //turn laser on
00061     wait(.1);
00062     //printf("Received %s", sent);
00063     _hokuyo.scanf("%s",&sent);
00064     
00065     if(!strcmp(sent, "L1")){  //if "L1\n" returned, successful
00066         sent[0] = '\0';
00067         _hokuyo.scanf("%s",&sent);
00068         
00069         if(!strcmp(sent, "0")){   // if "0\n" returned, successful
00070             //printf("Hokuyo Init SUCCESS\r\n");
00071             return 0;
00072         }        
00073         else{
00074             return -1;
00075         }
00076     }
00077     else{        
00078         //printf("ERROR - L1 not returned on initialization, received %s\r\n", sent); 
00079         return -2;
00080     }
00081 }
00082 
00083 //Uses the SCIP 1.0 : C-42-3320A protocol for scanner commands
00084 int Hokuyo::Read_Scan(float scan_deg, int clust_size)
00085 {
00086     char sent[MAXBUFSIZE];
00087     int N_read, i, j, k;
00088     int range_val, range_l, range_h;
00089     float minstep, maxstep;
00090     char temp[68];
00091     char *tok;
00092     int cluster;
00093     float angle_read;
00094     int stepnum;
00095     Timer t_delay;
00096 
00097     stepnum = 0;
00098     angle_read = -scan_deg/2.0;
00099 
00100     for(i=0;i<MAXBUFSIZE;i++)
00101         sent[i] = 0;
00102 
00103     while(this->_hokuyo.readable())     //clear the read buffer
00104         char c = this->_hokuyo.getc();
00105         
00106     minstep = 384 - (scan_deg/2.0)/.351;
00107     maxstep = 384 + (scan_deg/2.0)/.351;
00108 
00109     if(minstep < 0.0)            //-135 deg
00110         minstep = 0.0;
00111     if(maxstep > 768)            //+135 deg
00112         maxstep= 768;
00113     //int totalsteps = (int)(maxstep - minstep);
00114     cluster = clust_size;    //totalsteps/(int)scan_deg;
00115         
00116     strcpy(sent, "G");
00117     sprintf(temp, "%03d", (int)minstep);
00118     strcat(sent, temp);
00119     sprintf(temp, "%03d", (int)maxstep);
00120     strcat(sent, temp);
00121     sprintf(temp, "%02d", cluster);
00122     strcat(sent, temp);
00123     strcpy(temp, sent);
00124 
00125     this->_hokuyo.printf("%s\n",temp);   //fire laser
00126 //   serBputs("G12863214\n");   //fire laser
00127 
00128     t_delay.start();
00129     while(!this->_hokuyo.readable()){
00130         if(t_delay.read() > .13){
00131             printf("Timeout before receiving data packet\r\n");
00132             wait(2);
00133             return -1;      //timeout before receiving data packet
00134         }
00135     }
00136     t_delay.stop();
00137 //    wait(.13);
00138 
00139    N_read = this->lidar_read(sent, MAXBUFSIZE, .005);
00140     //pc.printf("number of bytes read = %d\r\n", N_read);
00141 
00142    tok = strtok(sent, "\n");        //extract first string
00143 //   pc.printf("%s was received back = should be the sent command!\r\n", tok);
00144    
00145    if(!strcmp(temp, tok))           //if sent command was received back
00146    {
00147        tok = strtok(NULL, "\n");    //extract error message
00148       if(atoi(tok) == 0)            //if error was 0
00149       {
00150                 //bytes read - command returned\n and error\n
00151             for(k=N_read-12;k>0;k-=65)  //64 data bytes plus '\n'
00152          {
00153               tok = strtok(NULL, "\n");  //get data string
00154               strcpy(temp, tok);
00155               j=strlen(temp);
00156 
00157               for(i=0;i<j;i++)
00158               {
00159                 range_h = ((temp[i] - 0x30) << 6);
00160                 i++;
00161                 range_l = (temp[i] - 0x30);
00162                 range_val = range_h + range_l;
00163                 this->dist_mm[stepnum] = range_val;
00164                 //printf("%3d %7.2f   %5d\r\n", stepnum++,angle_read, range_val);                    
00165                 
00166                 angle_read += (float)cluster * .351;
00167                 this->angle[stepnum] = angle_read;
00168                 stepnum++;
00169               }
00170          }  //read 64 bytes or remaining bytes at a time from returned data string
00171 //         printf("\r\n\r\n");
00172             this->numPts = stepnum;
00173             return 0;        //returns 0 on success
00174       } //no error
00175       else
00176       {
00177           printf("Error Taking Scan! Error returned.\r\n");
00178           return -2;
00179       }
00180    }    //command was received back
00181    else
00182    {
00183       printf("Error Taking Scan. No command returned!\r\n");
00184       return -1;
00185    }
00186 }
00187 
00188 /*
00189 //Working on function for auto baud detect and change to 750000 baud
00190 int Init_Hokuyo(int baudrate)
00191 {
00192     char sent[20];
00193     char temp[20];
00194     
00195     lidar.baud(19200);
00196     lidar.printf("L1\n");        //turn laser on
00197     wait(.1);
00198     lidar_read(sent, 3, .5);
00199     //lidar.scanf("%s",&sent);
00200     pc.printf("Received %s", sent);
00201     
00202     if(!strcmp(sent, "L1")){  //if "L1\n" returned, successful
00203         sent[0] = '\0';
00204         lidar.scanf("%s",&sent);
00205         
00206         if(!strcmp(sent, "0")){   // if "0\n" returned, successful
00207             pc.printf("Hokuyo Init SUCCESS\r\n");
00208             sprintf(temp, "S%06dFFFFFFF\n", baudrate);
00209             lidar.printf("%s", temp);
00210             
00211             lidar.scanf("%s\n",&sent);
00212             pc.printf("Received %s", sent);
00213             
00214             if(!strncmp(temp, sent, 14)){
00215                 pc.printf("Changing baud rate to %06d\r\n", baudrate);
00216                 lidar.baud(baudrate);        
00217             }
00218             
00219             return 0;
00220         }        
00221         else{
00222             return -1;
00223         }
00224     }
00225     else{
00226         lidar.baud(750000);
00227         lidar.printf("L1\n");        //turn laser on
00228         wait(.1);
00229         lidar_read(sent, 3, .5);
00230         pc.printf("Received %s", sent);
00231         
00232         if(!strcmp(sent, "L1")){  //if "L1\n" returned, successful
00233             sent[0] = '\0';
00234             lidar.scanf("%s",&sent);
00235             
00236             if(!strcmp(sent, "0")){   // if "0\n" returned, successful
00237                 pc.printf("Hokuyo Init SUCCESS\r\n");
00238                 sprintf(temp, "S%06dFFFFFFF\n", baudrate);
00239                 lidar.printf("%s", temp);
00240                 
00241                 lidar.scanf("%s\n",&sent);
00242                 pc.printf("Received %s", sent);
00243                 
00244                 if(!strncmp(temp, sent, 14)){
00245                     pc.printf("Changing baud rate to %06d\r\n", baudrate);
00246                     lidar.baud(baudrate);        
00247                 }
00248                 return 0;
00249             }        
00250             else{
00251                 return -1;
00252             }
00253         }        
00254         pc.printf("ERROR - L1 not returned on initialization, received %s\r\n", sent); 
00255         return -2;
00256     }
00257 }
00258 */