dotHR_URG
Dependencies: FatFileSystem TextLCD mbed
main.cpp
- Committer:
- higedura
- Date:
- 2015-09-29
- Revision:
- 2:22b200c374cd
- Parent:
- 1:52d3136e1a22
File content as of revision 2:22b200c374cd:
#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(p12); // button (green) Serial urg(p28, p27); // tx, rx //TextLCD lcd(p24, p26, p27, p28, p29, p30); // rs, e, d4-d7 // Local file system //LocalFileSystem local("local"); 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(115200); navi.baud(115200); urg.baud(19200); for( int i=0;i<18;i++ ){ i_pow=(double)i; pow2[i] = pow(2,i_pow); }; //FILE *fp = fopen("/local/out.txt", "w"); // Open "out.txt" on the local file system for writing // 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; wait(2); // 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]); } //for( int i=0;i<8;i++ ){ pc.putc(SCIP2[i]); } // Waiting SCIP 2.0 led2 = 1; wait(2); // 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); led3 = 1; wait(2); // Getting data //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]; } } // For test /* char buf_char; while(1){ buf_char = urg.getc(); pc.printf("%c", buf_char); } */ led4 = 1; //while(1){ while( stop==0 ){ //for(int iLoop=0;iLoop<3;iLoop++){ 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; } //for(int i=0;i<682;i++){ fprintf(fp, "%5d ", urg_depth[i]); } //fprintf(fp, "\n\r"); for(int i=0;i<682;i++){ pc.printf("%5d ", urg_depth[i]); } pc.printf("\n\r"); wait(3); // 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; } } //fclose(fp); led1 = 0; led2 = 0; led3 = 0; led4 = 0; }