hige dura
dotHR_URG
Dependencies: FatFileSystem TextLCD mbed
Diff: main.cpp
- Revision:
- 0:a1accf06b614
- Child:
- 1:52d3136e1a22
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Nov 30 12:34:22 2012 +0000
@@ -0,0 +1,232 @@
+#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
+InterruptIn urg_flag(p12);
+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};
+
+ 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
+ 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]; }
+ }
+
+ urg_flag.rise(&send_to_navi); // attach the address of the flip function to the rising edge
+
+ //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<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++ ){
+
+ 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;
+ }
+
+ }
+ /* 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]
+ 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; }
+
+ //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;
+ iaf_SL_print = iaf_SL;
+ iaf_SR = 0;
+ iaf_SL = 0;
+
+ }
+
+ }
+
+}
+
+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;
+
+}