Revision 0:a325ad337940, committed 2016-03-04

Comitter:

jebradshaw

Date:

Fri Mar 04 16:17:36 2016 +0000

Commit message:

Example program for interfacing to the Hokuyo URG-04LX lidar (2D range finder)

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/hokuyo.cpp	Fri Mar 04 16:17:36 2016 +0000
@@ -0,0 +1,258 @@
+// J Bradshaw 20160304
+
+#include "hokuyo.h"
+
+Hokuyo::Hokuyo(PinName tx, PinName rx) : _hokuyo(tx, rx) {
+    _hokuyo.baud(19200);
+}
+
+int Hokuyo::lidar_read(char *str, int numchars, float timeoutDelay){        
+    Timer t_Frame;
+    int i=0;        
+    int timeoutState=0;
+    float timeout;
+    
+    t_Frame.start();
+    
+    while(timeoutState != 3  && (i < numchars)){
+        switch(timeoutState){
+            case 0:
+                while(_hokuyo.readable()){ //if characters are in buffer, read them
+                    str[i++] = _hokuyo.getc();
+                    if(i == numchars){
+                        timeoutState = 3;
+                        break;   
+                    }
+                }
+                //if no characters in buffer, initiate timeout
+                timeoutState = 1;
+                break;
+            case 1:
+                timeout = t_Frame.read() + timeoutDelay;    //current time plus timeout time
+                timeoutState = 2;
+//                                              pc.printf("Timeout initiated %f\r\n", timeout);
+                break;
+            case 2:
+                if(_hokuyo.readable()){ //check buffer while timeout is running
+                    str[i++] = _hokuyo.getc();
+                    if(i == numchars){
+                        timeoutState = 3;
+                    }
+                    else{
+                        timeoutState = 0;
+                    }
+                    break;
+                }
+                if(t_Frame.read() >= timeout) //if timeout has elapsed, exit the while loop with state 3
+                    timeoutState = 3;
+                break;
+            default:
+                timeoutState = 0;
+        }//switch timeoutState                             
+    }//while timeoutState != 2
+    return i;   //return number of bytes read
+}
+
+int Hokuyo::Init(void)
+{
+    char sent[20];
+    
+    _hokuyo.printf("L1\n");        //turn laser on
+    wait(.1);
+    //printf("Received %s", sent);
+    _hokuyo.scanf("%s",&sent);
+    
+    if(!strcmp(sent, "L1")){  //if "L1\n" returned, successful
+        sent[0] = '\0';
+        _hokuyo.scanf("%s",&sent);
+        
+        if(!strcmp(sent, "0")){   // if "0\n" returned, successful
+            //printf("Hokuyo Init SUCCESS\r\n");
+            return 0;
+        }        
+        else{
+            return -1;
+        }
+    }
+    else{        
+        //printf("ERROR - L1 not returned on initialization, received %s\r\n", sent); 
+        return -2;
+    }
+}
+
+//Uses the SCIP 1.0 : C-42-3320A protocol for scanner commands
+int Hokuyo::Read_Scan(float scan_deg, int clust_size)
+{
+    char sent[MAXBUFSIZE];
+    int N_read, i, j, k;
+    int range_val, range_l, range_h;
+    float minstep, maxstep;
+    char temp[68];
+    char *tok;
+    int cluster;
+    float angle_read;
+    int stepnum;
+    Timer t_delay;
+
+    stepnum = 0;
+    angle_read = -scan_deg/2.0;
+
+    for(i=0;i<MAXBUFSIZE;i++)
+        sent[i] = 0;
+
+    while(this->_hokuyo.readable())     //clear the read buffer
+        char c = this->_hokuyo.getc();
+        
+    minstep = 384 - (scan_deg/2.0)/.351;
+    maxstep = 384 + (scan_deg/2.0)/.351;
+
+    if(minstep < 0.0)            //-135 deg
+        minstep = 0.0;
+    if(maxstep > 768)            //+135 deg
+        maxstep= 768;
+    //int totalsteps = (int)(maxstep - minstep);
+    cluster = clust_size;    //totalsteps/(int)scan_deg;
+        
+    strcpy(sent, "G");
+    sprintf(temp, "%03d", (int)minstep);
+    strcat(sent, temp);
+    sprintf(temp, "%03d", (int)maxstep);
+    strcat(sent, temp);
+    sprintf(temp, "%02d", cluster);
+    strcat(sent, temp);
+    strcpy(temp, sent);
+
+    this->_hokuyo.printf("%s\n",temp);   //fire laser
+//   serBputs("G12863214\n");   //fire laser
+
+    t_delay.start();
+    while(!this->_hokuyo.readable()){
+        if(t_delay.read() > .13){
+            printf("Timeout before receiving data packet\r\n");
+            wait(2);
+            return -1;      //timeout before receiving data packet
+        }
+    }
+    t_delay.stop();
+//    wait(.13);
+
+   N_read = this->lidar_read(sent, MAXBUFSIZE, .005);
+    //pc.printf("number of bytes read = %d\r\n", N_read);
+
+   tok = strtok(sent, "\n");        //extract first string
+//   pc.printf("%s was received back = should be the sent command!\r\n", tok);
+   
+   if(!strcmp(temp, tok))           //if sent command was received back
+   {
+       tok = strtok(NULL, "\n");    //extract error message
+      if(atoi(tok) == 0)            //if error was 0
+      {
+                //bytes read - command returned\n and error\n
+            for(k=N_read-12;k>0;k-=65)  //64 data bytes plus '\n'
+         {
+              tok = strtok(NULL, "\n");  //get data string
+              strcpy(temp, tok);
+              j=strlen(temp);
+
+              for(i=0;i<j;i++)
+              {
+                range_h = ((temp[i] - 0x30) << 6);
+                i++;
+                range_l = (temp[i] - 0x30);
+                range_val = range_h + range_l;
+                this->dist_mm[stepnum] = range_val;
+                //printf("%3d %7.2f   %5d\r\n", stepnum++,angle_read, range_val);                    
+                
+                angle_read += (float)cluster * .351;
+                this->angle[stepnum] = angle_read;
+                stepnum++;
+              }
+         }  //read 64 bytes or remaining bytes at a time from returned data string
+//         printf("\r\n\r\n");
+            this->numPts = stepnum;
+            return 0;        //returns 0 on success
+      } //no error
+      else
+      {
+          printf("Error Taking Scan! Error returned.\r\n");
+          return -2;
+      }
+   }    //command was received back
+   else
+   {
+      printf("Error Taking Scan. No command returned!\r\n");
+      return -1;
+   }
+}
+
+/*
+//Working on function for auto baud detect and change to 750000 baud
+int Init_Hokuyo(int baudrate)
+{
+    char sent[20];
+    char temp[20];
+    
+    lidar.baud(19200);
+    lidar.printf("L1\n");        //turn laser on
+    wait(.1);
+    lidar_read(sent, 3, .5);
+    //lidar.scanf("%s",&sent);
+    pc.printf("Received %s", sent);
+    
+    if(!strcmp(sent, "L1")){  //if "L1\n" returned, successful
+        sent[0] = '\0';
+        lidar.scanf("%s",&sent);
+        
+        if(!strcmp(sent, "0")){   // if "0\n" returned, successful
+            pc.printf("Hokuyo Init SUCCESS\r\n");
+            sprintf(temp, "S%06dFFFFFFF\n", baudrate);
+            lidar.printf("%s", temp);
+            
+            lidar.scanf("%s\n",&sent);
+            pc.printf("Received %s", sent);
+            
+            if(!strncmp(temp, sent, 14)){
+                pc.printf("Changing baud rate to %06d\r\n", baudrate);
+                lidar.baud(baudrate);        
+            }
+            
+            return 0;
+        }        
+        else{
+            return -1;
+        }
+    }
+    else{
+        lidar.baud(750000);
+        lidar.printf("L1\n");        //turn laser on
+        wait(.1);
+        lidar_read(sent, 3, .5);
+        pc.printf("Received %s", sent);
+        
+        if(!strcmp(sent, "L1")){  //if "L1\n" returned, successful
+            sent[0] = '\0';
+            lidar.scanf("%s",&sent);
+            
+            if(!strcmp(sent, "0")){   // if "0\n" returned, successful
+                pc.printf("Hokuyo Init SUCCESS\r\n");
+                sprintf(temp, "S%06dFFFFFFF\n", baudrate);
+                lidar.printf("%s", temp);
+                
+                lidar.scanf("%s\n",&sent);
+                pc.printf("Received %s", sent);
+                
+                if(!strncmp(temp, sent, 14)){
+                    pc.printf("Changing baud rate to %06d\r\n", baudrate);
+                    lidar.baud(baudrate);        
+                }
+                return 0;
+            }        
+            else{
+                return -1;
+            }
+        }        
+        pc.printf("ERROR - L1 not returned on initialization, received %s\r\n", sent); 
+        return -2;
+    }
+}
+*/
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/hokuyo.h	Fri Mar 04 16:17:36 2016 +0000
@@ -0,0 +1,26 @@
+// J Bradshaw 20160304
+#include "mbed.h"
+
+#ifndef HOKUYO_H
+#define HOKUYO_H
+
+#define MAXBUFSIZE 1024
+
+/**  A Hokuyo Lidar interface */
+class Hokuyo {
+public:
+    Hokuyo(PinName tx, PinName rx);
+    
+    int lidar_read(char *str, int numchars, float timeoutDelay);    //serial read function with timout (for large and non ASCII data strings)
+    int Read_Scan(float scan_deg, int clust_size);                  //send scan command and read data
+    int Init(void);                                                 //Initialize the Hokuyo lidar
+    
+    int numPts;                                                     //total number of points scanned from Read_Scan function
+    float angle[MAXBUFSIZE];                                        //angle array
+    int dist_mm[MAXBUFSIZE];                                        //distance array
+private:
+    Serial _hokuyo;                                                 //serial port connected to Hokuyo
+    
+};
+
+#endif
\ No newline at end of file