File content as of revision 0:a325ad337940:

// 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;
    }
}
*/