dotHR_URG
Dependencies: FatFileSystem TextLCD mbed
Diff: main.cpp
- Revision:
- 1:52d3136e1a22
- Parent:
- 0:a1accf06b614
- Child:
- 2:22b200c374cd
diff -r a1accf06b614 -r 52d3136e1a22 main.cpp --- a/main.cpp Fri Nov 30 12:34:22 2012 +0000 +++ b/main.cpp Wed Dec 05 13:56:13 2012 +0000 @@ -12,7 +12,6 @@ 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 -InterruptIn urg_flag(p12); Serial navi(p13, p14); // tx, rx DigitalIn stop(p17); // button (green) Serial urg(p28, p27); // tx, rx @@ -42,9 +41,6 @@ int pow2[18] = {0}; int urg_depth[682] = {0}; - int depth_sum_right = 0; - int depth_sum_left = 0; - // for IAF +-45deg 1600mm double range = 1600; double sin_iaf_angle = sin(0.352*pi/180); // mabiki nashi @@ -118,9 +114,7 @@ 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]; } } - - urg_flag.rise(&send_to_navi); // attach the address of the flip function to the rising edge - + /* //while(1){ while( stop==0 ){ @@ -137,13 +131,7 @@ 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<2110;i++ ){ pc.printf("%c", c_urg_data_buf[i]); } - pc.printf("\n\r\n\r"); - - for( int i=0;i<2046;i++ ){ pc.printf("%2d ", i_urg_data_buf[i]); } - pc.printf("\n\r\n\r"); - */ + for( int i=0;i<682;i++ ){ for( int j=0;j<3;j++ ){ @@ -159,38 +147,16 @@ } } - /* binary check - for( int j=0;j<3;j++ ){ - for( int k=0;k<6;k++ ){ - pc.printf("%d", b_urg_data[2-j][5-k]); - } - pc.printf(" "); - } - */ + 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; } - //pc.printf("%d ", urg_depth[i]); - } - //pc.printf("\r\n\r\n"); - - /* - // Full range - for( int i=0;i<341;i++ ){ - depth_sum_right += urg_depth[i]; - depth_sum_left += urg_depth[i+341]; + } - //pc.printf("\r\n%d %d\r\n\r\n", depth_sum_right, depth_sum_left); - // Initialization - for( int i=0;i<682;i++ ){ urg_depth[i] = 0; } - depth_sum_right = 0; - depth_sum_left = 0; - */ - + // IAF (+-45deg) (/1000000 is length conversion mm to m) - //for( int i=212;i<469;i++ ){ pc.printf("%d ", urg_depth[i]); } - //pc.printf("\r\n"); + 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] @@ -199,17 +165,8 @@ else{ iaf_delta = 500; } if( iaf_delta>800 ){ iaf_delta = 800; } if( iaf_delta<200 ){ iaf_delta = 200; } - - //pc.printf("%6.3f, %6.3f\r\n", iaf_SR, iaf_SL); - pc.printf("%4d\r\n", iaf_delta); - /* - if( urg_flag==1 ){ - navi.printf("%4d",iaf_delta); - //dotHR.printf("a"); - pc.printf("%4d, %4d\r\n", t, iaf_delta); - t = t+dt; - } - */ + + // Initialization for( int i=0;i<682;i++ ){ urg_depth[i] = 0; } iaf_SR_print = iaf_SR; @@ -220,13 +177,5 @@ } } - + */ } - -void send_to_navi() { - - navi.printf("%4d",iaf_delta); - pc.printf("%4d, %f, %f, %4d\r\n", t, iaf_SL_print, iaf_SR_print, iaf_delta); - t = t+dt; - -}