dotHR_URG
Dependencies: FatFileSystem TextLCD mbed
main.cpp@2:22b200c374cd, 2015-09-29 (annotated)
- Committer:
- higedura
- Date:
- Tue Sep 29 02:00:59 2015 +0000
- Revision:
- 2:22b200c374cd
- Parent:
- 1:52d3136e1a22
150929_URG
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
higedura | 0:a1accf06b614 | 1 | #include "mbed.h" |
higedura | 0:a1accf06b614 | 2 | #include "SDFileSystem.h" |
higedura | 0:a1accf06b614 | 3 | #include "ASCII.h" |
higedura | 0:a1accf06b614 | 4 | #include "URG.h" |
higedura | 0:a1accf06b614 | 5 | //#include "TextLCD.h" |
higedura | 0:a1accf06b614 | 6 | |
higedura | 0:a1accf06b614 | 7 | #define pi 3.14159265 |
higedura | 0:a1accf06b614 | 8 | |
higedura | 0:a1accf06b614 | 9 | DigitalOut led1(LED1); |
higedura | 0:a1accf06b614 | 10 | DigitalOut led2(LED2); |
higedura | 0:a1accf06b614 | 11 | DigitalOut led3(LED3); |
higedura | 0:a1accf06b614 | 12 | DigitalOut led4(LED4); |
higedura | 0:a1accf06b614 | 13 | Serial pc(USBTX, USBRX); // tx, rx |
higedura | 2:22b200c374cd | 14 | //SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board |
higedura | 0:a1accf06b614 | 15 | Serial navi(p13, p14); // tx, rx |
higedura | 2:22b200c374cd | 16 | DigitalIn stop(p12); // button (green) |
higedura | 0:a1accf06b614 | 17 | Serial urg(p28, p27); // tx, rx |
higedura | 0:a1accf06b614 | 18 | //TextLCD lcd(p24, p26, p27, p28, p29, p30); // rs, e, d4-d7 |
higedura | 0:a1accf06b614 | 19 | |
higedura | 2:22b200c374cd | 20 | // Local file system |
higedura | 2:22b200c374cd | 21 | //LocalFileSystem local("local"); |
higedura | 2:22b200c374cd | 22 | |
higedura | 0:a1accf06b614 | 23 | void send_to_navi(); |
higedura | 0:a1accf06b614 | 24 | |
higedura | 0:a1accf06b614 | 25 | int iaf_delta = 500; |
higedura | 0:a1accf06b614 | 26 | double iaf_SR_print = 0; |
higedura | 0:a1accf06b614 | 27 | double iaf_SL_print = 0; |
higedura | 0:a1accf06b614 | 28 | |
higedura | 0:a1accf06b614 | 29 | int t = -1; |
higedura | 0:a1accf06b614 | 30 | int dt = 1; |
higedura | 0:a1accf06b614 | 31 | |
higedura | 0:a1accf06b614 | 32 | int main() { |
higedura | 0:a1accf06b614 | 33 | |
higedura | 0:a1accf06b614 | 34 | //int t = -2; |
higedura | 0:a1accf06b614 | 35 | //int dt = 1; |
higedura | 0:a1accf06b614 | 36 | |
higedura | 0:a1accf06b614 | 37 | int flag = 0; |
higedura | 0:a1accf06b614 | 38 | |
higedura | 0:a1accf06b614 | 39 | char c_urg_data[3] = { 0, aA, 0 }; |
higedura | 0:a1accf06b614 | 40 | char c_urg_data_buf[2110] = {0}; |
higedura | 0:a1accf06b614 | 41 | int i_urg_data_buf[2046] = {0}; |
higedura | 0:a1accf06b614 | 42 | int b_urg_data[3][6] = {0}; |
higedura | 0:a1accf06b614 | 43 | int b_urg_data_sum[3] = {0}; |
higedura | 0:a1accf06b614 | 44 | int pow2[18] = {0}; |
higedura | 0:a1accf06b614 | 45 | int urg_depth[682] = {0}; |
higedura | 0:a1accf06b614 | 46 | |
higedura | 0:a1accf06b614 | 47 | // for IAF +-45deg 1600mm |
higedura | 0:a1accf06b614 | 48 | double range = 1600; |
higedura | 0:a1accf06b614 | 49 | double sin_iaf_angle = sin(0.352*pi/180); // mabiki nashi |
higedura | 0:a1accf06b614 | 50 | double iaf_SE = (range/1000)*(range/1000)*pi/4; |
higedura | 0:a1accf06b614 | 51 | double iaf_IL = 0; |
higedura | 0:a1accf06b614 | 52 | double iaf_gain = 1000; |
higedura | 0:a1accf06b614 | 53 | //int iaf_delta = 0; |
higedura | 0:a1accf06b614 | 54 | double iaf_SR = 0; |
higedura | 0:a1accf06b614 | 55 | double iaf_SL = 0; |
higedura | 0:a1accf06b614 | 56 | |
higedura | 0:a1accf06b614 | 57 | // Information Requirement 0<IR<1 |
higedura | 0:a1accf06b614 | 58 | double iaf_IR = 0.9; |
higedura | 0:a1accf06b614 | 59 | |
higedura | 0:a1accf06b614 | 60 | double i_pow = 0; |
higedura | 0:a1accf06b614 | 61 | |
higedura | 2:22b200c374cd | 62 | pc.baud(115200); |
higedura | 0:a1accf06b614 | 63 | navi.baud(115200); |
higedura | 0:a1accf06b614 | 64 | urg.baud(19200); |
higedura | 0:a1accf06b614 | 65 | |
higedura | 0:a1accf06b614 | 66 | for( int i=0;i<18;i++ ){ |
higedura | 0:a1accf06b614 | 67 | i_pow=(double)i; |
higedura | 0:a1accf06b614 | 68 | pow2[i] = pow(2,i_pow); |
higedura | 0:a1accf06b614 | 69 | }; |
higedura | 0:a1accf06b614 | 70 | |
higedura | 2:22b200c374cd | 71 | //FILE *fp = fopen("/local/out.txt", "w"); // Open "out.txt" on the local file system for writing |
higedura | 2:22b200c374cd | 72 | |
higedura | 0:a1accf06b614 | 73 | // Waiting start up URG |
higedura | 0:a1accf06b614 | 74 | //lcd.cls(); |
higedura | 0:a1accf06b614 | 75 | //lcd.printf("Waiting start up URG"); |
higedura | 0:a1accf06b614 | 76 | pc.printf("\n\r\n\rWaiting start up URG\n\r"); |
higedura | 2:22b200c374cd | 77 | led1 = 1; |
higedura | 2:22b200c374cd | 78 | wait(2); |
higedura | 0:a1accf06b614 | 79 | |
higedura | 0:a1accf06b614 | 80 | // SCIP 1.0 --> SCIP 2.0 |
higedura | 0:a1accf06b614 | 81 | //lcd.cls(); |
higedura | 0:a1accf06b614 | 82 | //lcd.printf("SCIP 2.0"); |
higedura | 0:a1accf06b614 | 83 | pc.printf("SCIP 2.0\n\r"); |
higedura | 0:a1accf06b614 | 84 | for( int i=0;i<8;i++ ){ urg.putc(SCIP2[i]); } |
higedura | 2:22b200c374cd | 85 | //for( int i=0;i<8;i++ ){ pc.putc(SCIP2[i]); } |
higedura | 0:a1accf06b614 | 86 | // Waiting SCIP 2.0 |
higedura | 2:22b200c374cd | 87 | led2 = 1; |
higedura | 2:22b200c374cd | 88 | wait(2); |
higedura | 0:a1accf06b614 | 89 | |
higedura | 0:a1accf06b614 | 90 | // Changing baudrate |
higedura | 0:a1accf06b614 | 91 | //lcd.cls(); |
higedura | 0:a1accf06b614 | 92 | //lcd.printf("Changing baudrate"); |
higedura | 0:a1accf06b614 | 93 | pc.printf("Changing baudrate\n\r"); |
higedura | 0:a1accf06b614 | 94 | for( int i=0;i<9;i++ ){ urg.putc(SS1152[i]); } |
higedura | 0:a1accf06b614 | 95 | urg.baud(115200); |
higedura | 2:22b200c374cd | 96 | led3 = 1; |
higedura | 2:22b200c374cd | 97 | wait(2); |
higedura | 0:a1accf06b614 | 98 | |
higedura | 0:a1accf06b614 | 99 | // Getting data |
higedura | 0:a1accf06b614 | 100 | //lcd.cls(); |
higedura | 0:a1accf06b614 | 101 | //lcd.printf("Getting data ..."); |
higedura | 0:a1accf06b614 | 102 | pc.printf("Getting data ...\n\r\n\r"); |
higedura | 0:a1accf06b614 | 103 | for( int i=0;i<16;i++ ){ urg.putc(MD[i]); } |
higedura | 0:a1accf06b614 | 104 | //for( int i=0;i<16;i++ ){ urg.putc(MD1[i]); } |
higedura | 0:a1accf06b614 | 105 | //for( int i=0;i<16;i++ ){ urg.putc(MD10[i]); } |
higedura | 0:a1accf06b614 | 106 | |
higedura | 0:a1accf06b614 | 107 | // Cutting first data |
higedura | 0:a1accf06b614 | 108 | while( flag==0 ){ |
higedura | 0:a1accf06b614 | 109 | c_urg_data[0] = urg.getc(); |
higedura | 0:a1accf06b614 | 110 | if( c_urg_data[0]==aLF && c_urg_data[1]==aLF ){ flag = 1; } |
higedura | 0:a1accf06b614 | 111 | for( int i=0;i<2;i++ ){ c_urg_data[i+1] = c_urg_data[i]; } |
higedura | 0:a1accf06b614 | 112 | } |
higedura | 2:22b200c374cd | 113 | |
higedura | 2:22b200c374cd | 114 | // For test |
higedura | 1:52d3136e1a22 | 115 | /* |
higedura | 2:22b200c374cd | 116 | char buf_char; |
higedura | 2:22b200c374cd | 117 | while(1){ |
higedura | 2:22b200c374cd | 118 | buf_char = urg.getc(); |
higedura | 2:22b200c374cd | 119 | pc.printf("%c", buf_char); |
higedura | 2:22b200c374cd | 120 | } |
higedura | 2:22b200c374cd | 121 | */ |
higedura | 2:22b200c374cd | 122 | |
higedura | 2:22b200c374cd | 123 | led4 = 1; |
higedura | 2:22b200c374cd | 124 | |
higedura | 0:a1accf06b614 | 125 | //while(1){ |
higedura | 0:a1accf06b614 | 126 | while( stop==0 ){ |
higedura | 2:22b200c374cd | 127 | //for(int iLoop=0;iLoop<3;iLoop++){ |
higedura | 0:a1accf06b614 | 128 | |
higedura | 0:a1accf06b614 | 129 | c_urg_data[2] = c_urg_data[1]; |
higedura | 0:a1accf06b614 | 130 | c_urg_data[1] = c_urg_data[0]; |
higedura | 0:a1accf06b614 | 131 | c_urg_data[0] = urg.getc(); |
higedura | 0:a1accf06b614 | 132 | |
higedura | 0:a1accf06b614 | 133 | if( c_urg_data[2]==a9 && c_urg_data[1]==a9 && c_urg_data[0]==ab ){ |
higedura | 0:a1accf06b614 | 134 | |
higedura | 0:a1accf06b614 | 135 | for( int i=0;i<7;i++ ){ c_urg_data[0] = urg.getc(); } |
higedura | 0:a1accf06b614 | 136 | |
higedura | 0:a1accf06b614 | 137 | for( int i=0;i<2110;i++ ){ c_urg_data_buf[i] = urg.getc(); } |
higedura | 0:a1accf06b614 | 138 | |
higedura | 0:a1accf06b614 | 139 | 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; } } |
higedura | 0:a1accf06b614 | 140 | |
higedura | 0:a1accf06b614 | 141 | for( int j=2046;j<2108;j++ ){ i_urg_data_buf[j-62] = (int)c_urg_data_buf[j]-48; } |
higedura | 1:52d3136e1a22 | 142 | |
higedura | 0:a1accf06b614 | 143 | for( int i=0;i<682;i++ ){ |
higedura | 0:a1accf06b614 | 144 | |
higedura | 0:a1accf06b614 | 145 | for( int j=0;j<3;j++ ){ |
higedura | 0:a1accf06b614 | 146 | |
higedura | 0:a1accf06b614 | 147 | for( int k=0;k<6;k++ ){ |
higedura | 0:a1accf06b614 | 148 | b_urg_data[2][k] = b_urg_data[1][k]; |
higedura | 0:a1accf06b614 | 149 | b_urg_data[1][k] = b_urg_data[0][k]; |
higedura | 0:a1accf06b614 | 150 | } |
higedura | 0:a1accf06b614 | 151 | |
higedura | 0:a1accf06b614 | 152 | for( int k=0;k<6;k++ ){ |
higedura | 0:a1accf06b614 | 153 | b_urg_data[0][k] = i_urg_data_buf[3*i+j]%2; |
higedura | 0:a1accf06b614 | 154 | i_urg_data_buf[3*i+j] = i_urg_data_buf[3*i+j]/2; |
higedura | 0:a1accf06b614 | 155 | } |
higedura | 0:a1accf06b614 | 156 | |
higedura | 0:a1accf06b614 | 157 | } |
higedura | 1:52d3136e1a22 | 158 | |
higedura | 0:a1accf06b614 | 159 | 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]; } } |
higedura | 0:a1accf06b614 | 160 | for( int j=0;j<3;j++ ){ urg_depth[i] += b_urg_data_sum[j]; } |
higedura | 0:a1accf06b614 | 161 | for( int j=0;j<3;j++ ){ b_urg_data_sum[j] = 0; } |
higedura | 0:a1accf06b614 | 162 | if( urg_depth[i]>range ){ urg_depth[i] = range; } |
higedura | 1:52d3136e1a22 | 163 | |
higedura | 0:a1accf06b614 | 164 | } |
higedura | 1:52d3136e1a22 | 165 | |
higedura | 0:a1accf06b614 | 166 | // IAF (+-45deg) (/1000000 is length conversion mm to m) |
higedura | 1:52d3136e1a22 | 167 | |
higedura | 0:a1accf06b614 | 168 | for( int i=212;i<340;i++ ){ iaf_SR += (double)urg_depth[i]*(double)urg_depth[i+1]/1000000*sin_iaf_angle/2; } |
higedura | 0:a1accf06b614 | 169 | for( int i=340;i<468;i++ ){ iaf_SL += (double)urg_depth[i]*(double)urg_depth[i+1]/1000000*sin_iaf_angle/2; } |
higedura | 0:a1accf06b614 | 170 | // iaf_delta: (10*delta + 500)[deg/s] |
higedura | 0:a1accf06b614 | 171 | iaf_IL = (iaf_SR+iaf_SL)/iaf_SE; |
higedura | 0:a1accf06b614 | 172 | if( iaf_IR-iaf_IL>0 ){ iaf_delta = (iaf_gain*(iaf_SR-iaf_SL))/iaf_SE+500; } |
higedura | 0:a1accf06b614 | 173 | else{ iaf_delta = 500; } |
higedura | 0:a1accf06b614 | 174 | if( iaf_delta>800 ){ iaf_delta = 800; } |
higedura | 0:a1accf06b614 | 175 | if( iaf_delta<200 ){ iaf_delta = 200; } |
higedura | 1:52d3136e1a22 | 176 | |
higedura | 2:22b200c374cd | 177 | //for(int i=0;i<682;i++){ fprintf(fp, "%5d ", urg_depth[i]); } |
higedura | 2:22b200c374cd | 178 | //fprintf(fp, "\n\r"); |
higedura | 2:22b200c374cd | 179 | for(int i=0;i<682;i++){ pc.printf("%5d ", urg_depth[i]); } |
higedura | 2:22b200c374cd | 180 | pc.printf("\n\r"); |
higedura | 2:22b200c374cd | 181 | wait(3); |
higedura | 2:22b200c374cd | 182 | |
higedura | 0:a1accf06b614 | 183 | // Initialization |
higedura | 0:a1accf06b614 | 184 | for( int i=0;i<682;i++ ){ urg_depth[i] = 0; } |
higedura | 0:a1accf06b614 | 185 | iaf_SR_print = iaf_SR; |
higedura | 0:a1accf06b614 | 186 | iaf_SL_print = iaf_SL; |
higedura | 0:a1accf06b614 | 187 | iaf_SR = 0; |
higedura | 0:a1accf06b614 | 188 | iaf_SL = 0; |
higedura | 0:a1accf06b614 | 189 | |
higedura | 0:a1accf06b614 | 190 | } |
higedura | 0:a1accf06b614 | 191 | |
higedura | 0:a1accf06b614 | 192 | } |
higedura | 2:22b200c374cd | 193 | |
higedura | 2:22b200c374cd | 194 | //fclose(fp); |
higedura | 2:22b200c374cd | 195 | led1 = 0; |
higedura | 2:22b200c374cd | 196 | led2 = 0; |
higedura | 2:22b200c374cd | 197 | led3 = 0; |
higedura | 2:22b200c374cd | 198 | led4 = 0; |
higedura | 2:22b200c374cd | 199 | |
higedura | 0:a1accf06b614 | 200 | } |