File content as of revision 1:52d3136e1a22:

#include "mbed.h"
#include "SDFileSystem.h"
#include "ASCII.h"
#include "URG.h"
//#include "TextLCD.h"

#define pi       3.14159265

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
Serial pc(USBTX, USBRX);                    // tx, rx
SDFileSystem sd(p5, p6, p7, p8, "sd");      // the pinout on the mbed Cool Components workshop board
Serial navi(p13, p14);                       // tx, rx
DigitalIn stop(p17);                        // button (green)
Serial urg(p28, p27);                       // tx, rx
//TextLCD lcd(p24, p26, p27, p28, p29, p30);  // rs, e, d4-d7

void send_to_navi();

int iaf_delta               =   500;
double iaf_SR_print         =   0;
double iaf_SL_print         =   0;

int t       =   -1;
int dt      =   1;

int main() {
    
    //int t       =   -2;
    //int dt      =   1;
    
    int flag    =   0;
    
    char c_urg_data[3]          =   { 0, aA, 0 };
    char c_urg_data_buf[2110]   =   {0};
    int i_urg_data_buf[2046]    =   {0};
    int b_urg_data[3][6]        =   {0};
    int b_urg_data_sum[3]       =   {0};
    int pow2[18]                =   {0};
    int urg_depth[682]          =   {0};
    
    // for IAF +-45deg 1600mm
    double range                =   1600;
    double sin_iaf_angle        =   sin(0.352*pi/180);              // mabiki nashi
    double iaf_SE               =   (range/1000)*(range/1000)*pi/4;
    double iaf_IL               =   0;
    double iaf_gain             =   1000;
    //int iaf_delta               =   0;
    double iaf_SR               =   0;
    double iaf_SL               =   0;
    
    // Information Requirement 0<IR<1
    double iaf_IR               =   0.9;
    
    double i_pow    =   0;
    
    pc.baud(921600);
    navi.baud(115200);
    urg.baud(19200);
    
    for( int i=0;i<18;i++ ){    
        i_pow=(double)i;
        pow2[i] = pow(2,i_pow);
    };
    
    // Waiting start up URG
    //lcd.cls();
    //lcd.printf("Waiting start up URG");
    pc.printf("\n\r\n\rWaiting start up URG\n\r");
    led1    =   1;  led2    =   1;  led3    =   1;  led4    =   1;
    wait(.5);    led4    =   0;
    wait(.5);    led3    =   0;
    wait(.5);    led2    =   0;
    wait(.5);    led1    =   0;    
    
    // SCIP 1.0 --> SCIP 2.0
    //lcd.cls();
    //lcd.printf("SCIP 2.0");
    pc.printf("SCIP 2.0\n\r");
    for( int i=0;i<8;i++ ){   urg.putc(SCIP2[i]);  }
    // Waiting SCIP 2.0
    led1    =   1;  led2    =   1;  led3    =   1;  led4    =   1;
    wait(.5);    led4    =   0;
    wait(.5);    led3    =   0;
    wait(.5);    led2    =   0;
    wait(.5);    led1    =   0;
    
    // Changing baudrate
    //lcd.cls();
    //lcd.printf("Changing baudrate");
    pc.printf("Changing baudrate\n\r");
    for( int i=0;i<9;i++ ){   urg.putc(SS1152[i]);  }
    urg.baud(115200);
    led1    =   1;  led2    =   1;  led3    =   1;  led4    =   1;
    wait(.5);    led4    =   0;
    wait(.5);    led3    =   0;
    wait(.5);    led2    =   0;
    wait(.5);    led1    =   0;
    
    // Getting data
    led1    =   1;  led4    =   1;
    //lcd.cls();
    //lcd.printf("Getting data ...");
    pc.printf("Getting data ...\n\r\n\r");
    for( int i=0;i<16;i++ ){  urg.putc(MD[i]);      }
    //for( int i=0;i<16;i++ ){  urg.putc(MD1[i]);     }
    //for( int i=0;i<16;i++ ){  urg.putc(MD10[i]);     }
    
    // Cutting first data
    while( flag==0 ){
        c_urg_data[0]    =   urg.getc();
        if( c_urg_data[0]==aLF && c_urg_data[1]==aLF ){   flag = 1;   }
        for( int i=0;i<2;i++ ){     c_urg_data[i+1] = c_urg_data[i];    }
    }
    /*
    //while(1){
    while( stop==0 ){
        
        c_urg_data[2] =   c_urg_data[1];
        c_urg_data[1] =   c_urg_data[0];
        c_urg_data[0] =   urg.getc();
        
        if( c_urg_data[2]==a9 && c_urg_data[1]==a9 && c_urg_data[0]==ab ){
            
            for( int i=0;i<7;i++ ){     c_urg_data[0] = urg.getc();                 }
            
            for( int i=0;i<2110;i++ ){  c_urg_data_buf[i] = urg.getc();             }
            
            for( int i=0;i<31;i++ ){    for( int j=66*i;j<66*i+64;j++ ){    i_urg_data_buf[j-2*i] = (int)c_urg_data_buf[j]-48;    }   }
            
            for( int j=2046;j<2108;j++ ){    i_urg_data_buf[j-62] = (int)c_urg_data_buf[j]-48;    }

            for( int i=0;i<682;i++ ){
                
                for( int j=0;j<3;j++ ){
                    
                    for( int k=0;k<6;k++ ){
                        b_urg_data[2][k] =   b_urg_data[1][k];
                        b_urg_data[1][k] =   b_urg_data[0][k];
                    }
                    
                    for( int k=0;k<6;k++ ){
                        b_urg_data[0][k] = i_urg_data_buf[3*i+j]%2;
                        i_urg_data_buf[3*i+j] = i_urg_data_buf[3*i+j]/2;
                    }
                    
                }

                for( int j=0;j<3;j++ ){     for( int k=0;k<6;k++ ){     b_urg_data_sum[j] += pow2[6*j+k]*b_urg_data[j][k];  }   }
                for( int j=0;j<3;j++ ){     urg_depth[i] += b_urg_data_sum[j];  }
                for( int j=0;j<3;j++ ){     b_urg_data_sum[j] = 0;              }
                if( urg_depth[i]>range ){   urg_depth[i] = range;                }
                
            }
                        
            // IAF (+-45deg) (/1000000 is length conversion mm to m)

            for( int i=212;i<340;i++ ){     iaf_SR += (double)urg_depth[i]*(double)urg_depth[i+1]/1000000*sin_iaf_angle/2;      }
            for( int i=340;i<468;i++ ){     iaf_SL += (double)urg_depth[i]*(double)urg_depth[i+1]/1000000*sin_iaf_angle/2;       }
            // iaf_delta: (10*delta + 500)[deg/s]
            iaf_IL = (iaf_SR+iaf_SL)/iaf_SE;
            if( iaf_IR-iaf_IL>0 ){  iaf_delta = (iaf_gain*(iaf_SR-iaf_SL))/iaf_SE+500;  }
            else{   iaf_delta = 500;    }
            if( iaf_delta>800 ){    iaf_delta = 800;    }
            if( iaf_delta<200 ){    iaf_delta = 200;    }
            

            // Initialization
            for( int i=0;i<682;i++ ){   urg_depth[i] = 0;   }
            iaf_SR_print    =   iaf_SR;
            iaf_SL_print    =   iaf_SL;
            iaf_SR          =   0;
            iaf_SL          =   0;
            
        }
        
    }
    */
}