Joseph Bradshaw / HOKUYO

Class Library for Hokuyo URG-04LX Range Finder (using SCIP1 communication specification)

Dependents:   mbed_Hokuyo_Example

This library interfaces the mbed to the Hokuyo 2D scanning laser range finder. The interface assumes the user has properly level shifted the serial communication lines to TTL (3.3 volt or 5 volt) levels from the Hokuyo URG-04LX RS-232 level UART. It should also be noted that the device requires about an amp of current on startup, then drops to a little less then a half amp when running. A sufficient 5 volt switching supply is my recommended power source. I am currently working on a function to auto detect the baud rate on initialization and change it to 750000. The heart of the library is a serial read function with timeout, which allows for capturing large data strings (non-ASCII or otherwise) of maximum length by using a state machine without having to alter the MODSERIAL library internal buffer sizes.

hokuyo.cpp@0:a325ad337940, 2016-03-04 (annotated)

Committer:
jebradshaw
Date:
Fri Mar 04 16:17:36 2016 +0000
Revision:
0:a325ad337940
Example program for interfacing to the Hokuyo URG-04LX lidar (2D range finder)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jebradshaw 0:a325ad337940 1 // J Bradshaw 20160304
jebradshaw 0:a325ad337940 2
jebradshaw 0:a325ad337940 3 #include "hokuyo.h"
jebradshaw 0:a325ad337940 4
jebradshaw 0:a325ad337940 5 Hokuyo::Hokuyo(PinName tx, PinName rx) : _hokuyo(tx, rx) {
jebradshaw 0:a325ad337940 6 _hokuyo.baud(19200);
jebradshaw 0:a325ad337940 7 }
jebradshaw 0:a325ad337940 8
jebradshaw 0:a325ad337940 9 int Hokuyo::lidar_read(char *str, int numchars, float timeoutDelay){
jebradshaw 0:a325ad337940 10 Timer t_Frame;
jebradshaw 0:a325ad337940 11 int i=0;
jebradshaw 0:a325ad337940 12 int timeoutState=0;
jebradshaw 0:a325ad337940 13 float timeout;
jebradshaw 0:a325ad337940 14
jebradshaw 0:a325ad337940 15 t_Frame.start();
jebradshaw 0:a325ad337940 16
jebradshaw 0:a325ad337940 17 while(timeoutState != 3 && (i < numchars)){
jebradshaw 0:a325ad337940 18 switch(timeoutState){
jebradshaw 0:a325ad337940 19 case 0:
jebradshaw 0:a325ad337940 20 while(_hokuyo.readable()){ //if characters are in buffer, read them
jebradshaw 0:a325ad337940 21 str[i++] = _hokuyo.getc();
jebradshaw 0:a325ad337940 22 if(i == numchars){
jebradshaw 0:a325ad337940 23 timeoutState = 3;
jebradshaw 0:a325ad337940 24 break;
jebradshaw 0:a325ad337940 25 }
jebradshaw 0:a325ad337940 26 }
jebradshaw 0:a325ad337940 27 //if no characters in buffer, initiate timeout
jebradshaw 0:a325ad337940 28 timeoutState = 1;
jebradshaw 0:a325ad337940 29 break;
jebradshaw 0:a325ad337940 30 case 1:
jebradshaw 0:a325ad337940 31 timeout = t_Frame.read() + timeoutDelay; //current time plus timeout time
jebradshaw 0:a325ad337940 32 timeoutState = 2;
jebradshaw 0:a325ad337940 33 // pc.printf("Timeout initiated %f\r\n", timeout);
jebradshaw 0:a325ad337940 34 break;
jebradshaw 0:a325ad337940 35 case 2:
jebradshaw 0:a325ad337940 36 if(_hokuyo.readable()){ //check buffer while timeout is running
jebradshaw 0:a325ad337940 37 str[i++] = _hokuyo.getc();
jebradshaw 0:a325ad337940 38 if(i == numchars){
jebradshaw 0:a325ad337940 39 timeoutState = 3;
jebradshaw 0:a325ad337940 40 }
jebradshaw 0:a325ad337940 41 else{
jebradshaw 0:a325ad337940 42 timeoutState = 0;
jebradshaw 0:a325ad337940 43 }
jebradshaw 0:a325ad337940 44 break;
jebradshaw 0:a325ad337940 45 }
jebradshaw 0:a325ad337940 46 if(t_Frame.read() >= timeout) //if timeout has elapsed, exit the while loop with state 3
jebradshaw 0:a325ad337940 47 timeoutState = 3;
jebradshaw 0:a325ad337940 48 break;
jebradshaw 0:a325ad337940 49 default:
jebradshaw 0:a325ad337940 50 timeoutState = 0;
jebradshaw 0:a325ad337940 51 }//switch timeoutState
jebradshaw 0:a325ad337940 52 }//while timeoutState != 2
jebradshaw 0:a325ad337940 53 return i; //return number of bytes read
jebradshaw 0:a325ad337940 54 }
jebradshaw 0:a325ad337940 55
jebradshaw 0:a325ad337940 56 int Hokuyo::Init(void)
jebradshaw 0:a325ad337940 57 {
jebradshaw 0:a325ad337940 58 char sent[20];
jebradshaw 0:a325ad337940 59
jebradshaw 0:a325ad337940 60 _hokuyo.printf("L1\n"); //turn laser on
jebradshaw 0:a325ad337940 61 wait(.1);
jebradshaw 0:a325ad337940 62 //printf("Received %s", sent);
jebradshaw 0:a325ad337940 63 _hokuyo.scanf("%s",&sent);
jebradshaw 0:a325ad337940 64
jebradshaw 0:a325ad337940 65 if(!strcmp(sent, "L1")){ //if "L1\n" returned, successful
jebradshaw 0:a325ad337940 66 sent[0] = '\0';
jebradshaw 0:a325ad337940 67 _hokuyo.scanf("%s",&sent);
jebradshaw 0:a325ad337940 68
jebradshaw 0:a325ad337940 69 if(!strcmp(sent, "0")){ // if "0\n" returned, successful
jebradshaw 0:a325ad337940 70 //printf("Hokuyo Init SUCCESS\r\n");
jebradshaw 0:a325ad337940 71 return 0;
jebradshaw 0:a325ad337940 72 }
jebradshaw 0:a325ad337940 73 else{
jebradshaw 0:a325ad337940 74 return -1;
jebradshaw 0:a325ad337940 75 }
jebradshaw 0:a325ad337940 76 }
jebradshaw 0:a325ad337940 77 else{
jebradshaw 0:a325ad337940 78 //printf("ERROR - L1 not returned on initialization, received %s\r\n", sent);
jebradshaw 0:a325ad337940 79 return -2;
jebradshaw 0:a325ad337940 80 }
jebradshaw 0:a325ad337940 81 }
jebradshaw 0:a325ad337940 82
jebradshaw 0:a325ad337940 83 //Uses the SCIP 1.0 : C-42-3320A protocol for scanner commands
jebradshaw 0:a325ad337940 84 int Hokuyo::Read_Scan(float scan_deg, int clust_size)
jebradshaw 0:a325ad337940 85 {
jebradshaw 0:a325ad337940 86 char sent[MAXBUFSIZE];
jebradshaw 0:a325ad337940 87 int N_read, i, j, k;
jebradshaw 0:a325ad337940 88 int range_val, range_l, range_h;
jebradshaw 0:a325ad337940 89 float minstep, maxstep;
jebradshaw 0:a325ad337940 90 char temp[68];
jebradshaw 0:a325ad337940 91 char *tok;
jebradshaw 0:a325ad337940 92 int cluster;
jebradshaw 0:a325ad337940 93 float angle_read;
jebradshaw 0:a325ad337940 94 int stepnum;
jebradshaw 0:a325ad337940 95 Timer t_delay;
jebradshaw 0:a325ad337940 96
jebradshaw 0:a325ad337940 97 stepnum = 0;
jebradshaw 0:a325ad337940 98 angle_read = -scan_deg/2.0;
jebradshaw 0:a325ad337940 99
jebradshaw 0:a325ad337940 100 for(i=0;i<MAXBUFSIZE;i++)
jebradshaw 0:a325ad337940 101 sent[i] = 0;
jebradshaw 0:a325ad337940 102
jebradshaw 0:a325ad337940 103 while(this->_hokuyo.readable()) //clear the read buffer
jebradshaw 0:a325ad337940 104 char c = this->_hokuyo.getc();
jebradshaw 0:a325ad337940 105
jebradshaw 0:a325ad337940 106 minstep = 384 - (scan_deg/2.0)/.351;
jebradshaw 0:a325ad337940 107 maxstep = 384 + (scan_deg/2.0)/.351;
jebradshaw 0:a325ad337940 108
jebradshaw 0:a325ad337940 109 if(minstep < 0.0) //-135 deg
jebradshaw 0:a325ad337940 110 minstep = 0.0;
jebradshaw 0:a325ad337940 111 if(maxstep > 768) //+135 deg
jebradshaw 0:a325ad337940 112 maxstep= 768;
jebradshaw 0:a325ad337940 113 //int totalsteps = (int)(maxstep - minstep);
jebradshaw 0:a325ad337940 114 cluster = clust_size; //totalsteps/(int)scan_deg;
jebradshaw 0:a325ad337940 115
jebradshaw 0:a325ad337940 116 strcpy(sent, "G");
jebradshaw 0:a325ad337940 117 sprintf(temp, "%03d", (int)minstep);
jebradshaw 0:a325ad337940 118 strcat(sent, temp);
jebradshaw 0:a325ad337940 119 sprintf(temp, "%03d", (int)maxstep);
jebradshaw 0:a325ad337940 120 strcat(sent, temp);
jebradshaw 0:a325ad337940 121 sprintf(temp, "%02d", cluster);
jebradshaw 0:a325ad337940 122 strcat(sent, temp);
jebradshaw 0:a325ad337940 123 strcpy(temp, sent);
jebradshaw 0:a325ad337940 124
jebradshaw 0:a325ad337940 125 this->_hokuyo.printf("%s\n",temp); //fire laser
jebradshaw 0:a325ad337940 126 // serBputs("G12863214\n"); //fire laser
jebradshaw 0:a325ad337940 127
jebradshaw 0:a325ad337940 128 t_delay.start();
jebradshaw 0:a325ad337940 129 while(!this->_hokuyo.readable()){
jebradshaw 0:a325ad337940 130 if(t_delay.read() > .13){
jebradshaw 0:a325ad337940 131 printf("Timeout before receiving data packet\r\n");
jebradshaw 0:a325ad337940 132 wait(2);
jebradshaw 0:a325ad337940 133 return -1; //timeout before receiving data packet
jebradshaw 0:a325ad337940 134 }
jebradshaw 0:a325ad337940 135 }
jebradshaw 0:a325ad337940 136 t_delay.stop();
jebradshaw 0:a325ad337940 137 // wait(.13);
jebradshaw 0:a325ad337940 138
jebradshaw 0:a325ad337940 139 N_read = this->lidar_read(sent, MAXBUFSIZE, .005);
jebradshaw 0:a325ad337940 140 //pc.printf("number of bytes read = %d\r\n", N_read);
jebradshaw 0:a325ad337940 141
jebradshaw 0:a325ad337940 142 tok = strtok(sent, "\n"); //extract first string
jebradshaw 0:a325ad337940 143 // pc.printf("%s was received back = should be the sent command!\r\n", tok);
jebradshaw 0:a325ad337940 144
jebradshaw 0:a325ad337940 145 if(!strcmp(temp, tok)) //if sent command was received back
jebradshaw 0:a325ad337940 146 {
jebradshaw 0:a325ad337940 147 tok = strtok(NULL, "\n"); //extract error message
jebradshaw 0:a325ad337940 148 if(atoi(tok) == 0) //if error was 0
jebradshaw 0:a325ad337940 149 {
jebradshaw 0:a325ad337940 150 //bytes read - command returned\n and error\n
jebradshaw 0:a325ad337940 151 for(k=N_read-12;k>0;k-=65) //64 data bytes plus '\n'
jebradshaw 0:a325ad337940 152 {
jebradshaw 0:a325ad337940 153 tok = strtok(NULL, "\n"); //get data string
jebradshaw 0:a325ad337940 154 strcpy(temp, tok);
jebradshaw 0:a325ad337940 155 j=strlen(temp);
jebradshaw 0:a325ad337940 156
jebradshaw 0:a325ad337940 157 for(i=0;i<j;i++)
jebradshaw 0:a325ad337940 158 {
jebradshaw 0:a325ad337940 159 range_h = ((temp[i] - 0x30) << 6);
jebradshaw 0:a325ad337940 160 i++;
jebradshaw 0:a325ad337940 161 range_l = (temp[i] - 0x30);
jebradshaw 0:a325ad337940 162 range_val = range_h + range_l;
jebradshaw 0:a325ad337940 163 this->dist_mm[stepnum] = range_val;
jebradshaw 0:a325ad337940 164 //printf("%3d %7.2f %5d\r\n", stepnum++,angle_read, range_val);
jebradshaw 0:a325ad337940 165
jebradshaw 0:a325ad337940 166 angle_read += (float)cluster * .351;
jebradshaw 0:a325ad337940 167 this->angle[stepnum] = angle_read;
jebradshaw 0:a325ad337940 168 stepnum++;
jebradshaw 0:a325ad337940 169 }
jebradshaw 0:a325ad337940 170 } //read 64 bytes or remaining bytes at a time from returned data string
jebradshaw 0:a325ad337940 171 // printf("\r\n\r\n");
jebradshaw 0:a325ad337940 172 this->numPts = stepnum;
jebradshaw 0:a325ad337940 173 return 0; //returns 0 on success
jebradshaw 0:a325ad337940 174 } //no error
jebradshaw 0:a325ad337940 175 else
jebradshaw 0:a325ad337940 176 {
jebradshaw 0:a325ad337940 177 printf("Error Taking Scan! Error returned.\r\n");
jebradshaw 0:a325ad337940 178 return -2;
jebradshaw 0:a325ad337940 179 }
jebradshaw 0:a325ad337940 180 } //command was received back
jebradshaw 0:a325ad337940 181 else
jebradshaw 0:a325ad337940 182 {
jebradshaw 0:a325ad337940 183 printf("Error Taking Scan. No command returned!\r\n");
jebradshaw 0:a325ad337940 184 return -1;
jebradshaw 0:a325ad337940 185 }
jebradshaw 0:a325ad337940 186 }
jebradshaw 0:a325ad337940 187
jebradshaw 0:a325ad337940 188 /*
jebradshaw 0:a325ad337940 189 //Working on function for auto baud detect and change to 750000 baud
jebradshaw 0:a325ad337940 190 int Init_Hokuyo(int baudrate)
jebradshaw 0:a325ad337940 191 {
jebradshaw 0:a325ad337940 192 char sent[20];
jebradshaw 0:a325ad337940 193 char temp[20];
jebradshaw 0:a325ad337940 194
jebradshaw 0:a325ad337940 195 lidar.baud(19200);
jebradshaw 0:a325ad337940 196 lidar.printf("L1\n"); //turn laser on
jebradshaw 0:a325ad337940 197 wait(.1);
jebradshaw 0:a325ad337940 198 lidar_read(sent, 3, .5);
jebradshaw 0:a325ad337940 199 //lidar.scanf("%s",&sent);
jebradshaw 0:a325ad337940 200 pc.printf("Received %s", sent);
jebradshaw 0:a325ad337940 201
jebradshaw 0:a325ad337940 202 if(!strcmp(sent, "L1")){ //if "L1\n" returned, successful
jebradshaw 0:a325ad337940 203 sent[0] = '\0';
jebradshaw 0:a325ad337940 204 lidar.scanf("%s",&sent);
jebradshaw 0:a325ad337940 205
jebradshaw 0:a325ad337940 206 if(!strcmp(sent, "0")){ // if "0\n" returned, successful
jebradshaw 0:a325ad337940 207 pc.printf("Hokuyo Init SUCCESS\r\n");
jebradshaw 0:a325ad337940 208 sprintf(temp, "S%06dFFFFFFF\n", baudrate);
jebradshaw 0:a325ad337940 209 lidar.printf("%s", temp);
jebradshaw 0:a325ad337940 210
jebradshaw 0:a325ad337940 211 lidar.scanf("%s\n",&sent);
jebradshaw 0:a325ad337940 212 pc.printf("Received %s", sent);
jebradshaw 0:a325ad337940 213
jebradshaw 0:a325ad337940 214 if(!strncmp(temp, sent, 14)){
jebradshaw 0:a325ad337940 215 pc.printf("Changing baud rate to %06d\r\n", baudrate);
jebradshaw 0:a325ad337940 216 lidar.baud(baudrate);
jebradshaw 0:a325ad337940 217 }
jebradshaw 0:a325ad337940 218
jebradshaw 0:a325ad337940 219 return 0;
jebradshaw 0:a325ad337940 220 }
jebradshaw 0:a325ad337940 221 else{
jebradshaw 0:a325ad337940 222 return -1;
jebradshaw 0:a325ad337940 223 }
jebradshaw 0:a325ad337940 224 }
jebradshaw 0:a325ad337940 225 else{
jebradshaw 0:a325ad337940 226 lidar.baud(750000);
jebradshaw 0:a325ad337940 227 lidar.printf("L1\n"); //turn laser on
jebradshaw 0:a325ad337940 228 wait(.1);
jebradshaw 0:a325ad337940 229 lidar_read(sent, 3, .5);
jebradshaw 0:a325ad337940 230 pc.printf("Received %s", sent);
jebradshaw 0:a325ad337940 231
jebradshaw 0:a325ad337940 232 if(!strcmp(sent, "L1")){ //if "L1\n" returned, successful
jebradshaw 0:a325ad337940 233 sent[0] = '\0';
jebradshaw 0:a325ad337940 234 lidar.scanf("%s",&sent);
jebradshaw 0:a325ad337940 235
jebradshaw 0:a325ad337940 236 if(!strcmp(sent, "0")){ // if "0\n" returned, successful
jebradshaw 0:a325ad337940 237 pc.printf("Hokuyo Init SUCCESS\r\n");
jebradshaw 0:a325ad337940 238 sprintf(temp, "S%06dFFFFFFF\n", baudrate);
jebradshaw 0:a325ad337940 239 lidar.printf("%s", temp);
jebradshaw 0:a325ad337940 240
jebradshaw 0:a325ad337940 241 lidar.scanf("%s\n",&sent);
jebradshaw 0:a325ad337940 242 pc.printf("Received %s", sent);
jebradshaw 0:a325ad337940 243
jebradshaw 0:a325ad337940 244 if(!strncmp(temp, sent, 14)){
jebradshaw 0:a325ad337940 245 pc.printf("Changing baud rate to %06d\r\n", baudrate);
jebradshaw 0:a325ad337940 246 lidar.baud(baudrate);
jebradshaw 0:a325ad337940 247 }
jebradshaw 0:a325ad337940 248 return 0;
jebradshaw 0:a325ad337940 249 }
jebradshaw 0:a325ad337940 250 else{
jebradshaw 0:a325ad337940 251 return -1;
jebradshaw 0:a325ad337940 252 }
jebradshaw 0:a325ad337940 253 }
jebradshaw 0:a325ad337940 254 pc.printf("ERROR - L1 not returned on initialization, received %s\r\n", sent);
jebradshaw 0:a325ad337940 255 return -2;
jebradshaw 0:a325ad337940 256 }
jebradshaw 0:a325ad337940 257 }
jebradshaw 0:a325ad337940 258 */