hige dura / Mbed 2 deprecated urgIaf

This code is Information Amount Feedback (IAF) with HOKUYO URG.

Dependencies:   mbed

main.cpp@0:d2ef19170c3d, 2015-01-16 (annotated)

Committer:
higedura
Date:
Fri Jan 16 09:51:32 2015 +0000
Revision:
0:d2ef19170c3d
Information amount feedback with URG

Who changed what in which revision?

UserRevisionLine numberNew contents of line
higedura 0:d2ef19170c3d 1 #include "mbed.h"
higedura 0:d2ef19170c3d 2 #include "ASCII.h"
higedura 0:d2ef19170c3d 3 #include "URG.h"
higedura 0:d2ef19170c3d 4
higedura 0:d2ef19170c3d 5 #define pi 3.14159265
higedura 0:d2ef19170c3d 6 /*
higedura 0:d2ef19170c3d 7 // full range
higedura 0:d2ef19170c3d 8 #define N_raw_data 2110
higedura 0:d2ef19170c3d 9 #define N_usable_data 2046
higedura 0:d2ef19170c3d 10 #define N_laser 682
higedura 0:d2ef19170c3d 11 */
higedura 0:d2ef19170c3d 12 /*
higedura 0:d2ef19170c3d 13 // 90 deg
higedura 0:d2ef19170c3d 14 #define N_raw_data 797
higedura 0:d2ef19170c3d 15 #define N_usable_data 771
higedura 0:d2ef19170c3d 16 #define N_laser 257
higedura 0:d2ef19170c3d 17 */
higedura 0:d2ef19170c3d 18 // 180 deg
higedura 0:d2ef19170c3d 19 #define N_raw_data 1589
higedura 0:d2ef19170c3d 20 #define N_usable_data 1539
higedura 0:d2ef19170c3d 21 #define N_laser 513
higedura 0:d2ef19170c3d 22
higedura 0:d2ef19170c3d 23 Serial pc(USBTX, USBRX);
higedura 0:d2ef19170c3d 24 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
higedura 0:d2ef19170c3d 25
higedura 0:d2ef19170c3d 26 DigitalOut led1(LED1);
higedura 0:d2ef19170c3d 27 DigitalOut led2(LED2);
higedura 0:d2ef19170c3d 28 DigitalOut led3(LED3);
higedura 0:d2ef19170c3d 29 DigitalOut led4(LED4);
higedura 0:d2ef19170c3d 30
higedura 0:d2ef19170c3d 31 Serial yalHack(p9,p10); // tx, rx
higedura 0:d2ef19170c3d 32
higedura 0:d2ef19170c3d 33 DigitalIn stop(p12);
higedura 0:d2ef19170c3d 34
higedura 0:d2ef19170c3d 35 Serial urg(p28, p27); // tx, rx
higedura 0:d2ef19170c3d 36
higedura 0:d2ef19170c3d 37 Timer t_system;
higedura 0:d2ef19170c3d 38
higedura 0:d2ef19170c3d 39 int main() {
higedura 0:d2ef19170c3d 40
higedura 0:d2ef19170c3d 41 t_system.start();
higedura 0:d2ef19170c3d 42
higedura 0:d2ef19170c3d 43 double range_iaf_max = 2; // [mm]
higedura 0:d2ef19170c3d 44 double angle_of_view = 180; // [deg]
higedura 0:d2ef19170c3d 45 double gain_iaf = 1;
higedura 0:d2ef19170c3d 46 double area_focused = range_iaf_max * range_iaf_max * pi * angle_of_view / 360;
higedura 0:d2ef19170c3d 47
higedura 0:d2ef19170c3d 48 int laser_center = (N_laser-1)/2;
higedura 0:d2ef19170c3d 49
higedura 0:d2ef19170c3d 50 double steering_angle_rad = 0;
higedura 0:d2ef19170c3d 51 double steering_angle_deg = 0;
higedura 0:d2ef19170c3d 52 int i_steering_angle_send_buf1 = 0;
higedura 0:d2ef19170c3d 53 int i_steering_angle_send_buf2[5] = {0};
higedura 0:d2ef19170c3d 54 char ascii_steering_angle_send[5] = {0};
higedura 0:d2ef19170c3d 55
higedura 0:d2ef19170c3d 56 pc.baud(921600);
higedura 0:d2ef19170c3d 57 yalHack.baud(921600);
higedura 0:d2ef19170c3d 58 urg.baud(19200);
higedura 0:d2ef19170c3d 59
higedura 0:d2ef19170c3d 60 char c_urg_data[3] = { 0, aA, 0 };
higedura 0:d2ef19170c3d 61 char c_urg_data_buf[N_raw_data] = {0};
higedura 0:d2ef19170c3d 62 int i_urg_data_buf[N_usable_data] = {0};
higedura 0:d2ef19170c3d 63 int b_urg_data[3][6] = {0};
higedura 0:d2ef19170c3d 64 int b_urg_data_sum[3] = {0};
higedura 0:d2ef19170c3d 65 int pow2[18] = {0};
higedura 0:d2ef19170c3d 66 int i_urg_depth[N_laser] = {0};
higedura 0:d2ef19170c3d 67
higedura 0:d2ef19170c3d 68 double urg_depth[N_laser] = {0};
higedura 0:d2ef19170c3d 69 //double iaf_triangle[N_laser-1] = {0};
higedura 0:d2ef19170c3d 70 double iaf_front = 0;
higedura 0:d2ef19170c3d 71 double iaf_back = 0;
higedura 0:d2ef19170c3d 72 double sin_iaf_angle = sin(0.352*pi/180);
higedura 0:d2ef19170c3d 73
higedura 0:d2ef19170c3d 74 double i_pow = 0;
higedura 0:d2ef19170c3d 75
higedura 0:d2ef19170c3d 76 for( int i=0;i<18;i++ ){
higedura 0:d2ef19170c3d 77 i_pow=(double)i;
higedura 0:d2ef19170c3d 78 pow2[i] = pow(2,i_pow);
higedura 0:d2ef19170c3d 79 };
higedura 0:d2ef19170c3d 80
higedura 0:d2ef19170c3d 81 FILE *fp = fopen("/local/out.txt", "w"); // Open "out.txt" on the local file system for writing
higedura 0:d2ef19170c3d 82
higedura 0:d2ef19170c3d 83 // Waiting start up URG
higedura 0:d2ef19170c3d 84 pc.printf("\n\r\n\rWaiting start up URG\n\r");
higedura 0:d2ef19170c3d 85 //xbee.printf("\n\r\n\rWaiting start up URG\n\r");
higedura 0:d2ef19170c3d 86 led1 = 1; led2 = 1; led3 = 1; led4 = 1;
higedura 0:d2ef19170c3d 87 wait(1); led4 = 0;
higedura 0:d2ef19170c3d 88 wait(1); led3 = 0;
higedura 0:d2ef19170c3d 89 wait(1); led2 = 0;
higedura 0:d2ef19170c3d 90 wait(1); led1 = 0;
higedura 0:d2ef19170c3d 91
higedura 0:d2ef19170c3d 92 // SCIP 1.0 --> SCIP 2.0
higedura 0:d2ef19170c3d 93 pc.printf("SCIP 2.0\n\r");
higedura 0:d2ef19170c3d 94 //xbee.printf("SCIP 2.0\n\r");
higedura 0:d2ef19170c3d 95 for( int i=0;i<8;i++ ){ urg.putc(SCIP2[i]); }
higedura 0:d2ef19170c3d 96 // Waiting SCIP 2.0
higedura 0:d2ef19170c3d 97 led1 = 1; led2 = 1; led3 = 1; led4 = 1;
higedura 0:d2ef19170c3d 98 wait(.25); led4 = 0;
higedura 0:d2ef19170c3d 99 wait(.25); led3 = 0;
higedura 0:d2ef19170c3d 100 wait(.25); led2 = 0;
higedura 0:d2ef19170c3d 101 wait(.25); led1 = 0;
higedura 0:d2ef19170c3d 102
higedura 0:d2ef19170c3d 103 // Changing baudrate
higedura 0:d2ef19170c3d 104 pc.printf("Changing baudrate\n\r");
higedura 0:d2ef19170c3d 105 //xbee.printf("Changing baudrate\n\r");
higedura 0:d2ef19170c3d 106 for( int i=0;i<9;i++ ){ urg.putc(SS1152[i]); }
higedura 0:d2ef19170c3d 107 urg.baud(115200);
higedura 0:d2ef19170c3d 108 led1 = 1; led2 = 1; led3 = 1; led4 = 1;
higedura 0:d2ef19170c3d 109 wait(.25); led4 = 0;
higedura 0:d2ef19170c3d 110 wait(.25); led3 = 0;
higedura 0:d2ef19170c3d 111 wait(.25); led2 = 0;
higedura 0:d2ef19170c3d 112 wait(.25); led1 = 0;
higedura 0:d2ef19170c3d 113
higedura 0:d2ef19170c3d 114 // Getting data
higedura 0:d2ef19170c3d 115 // MD | range (3-digit deg) | N of assembled steps (2-digit) | N of thinned scans (1-digit) | N of transmissions (2-digit) |
higedura 0:d2ef19170c3d 116 // 0FR means full range (= 239.4 degs).
higedura 0:d2ef19170c3d 117 // i.e.) MD_09001301 -> range = 90 degs, scanning full steps, scanning every 3rd, transmitting once
higedura 0:d2ef19170c3d 118 led1 = 1; led4 = 1;
higedura 0:d2ef19170c3d 119 for( int i=0;i<16;i++ ){ urg.putc(MD_18001000[i]); }
higedura 0:d2ef19170c3d 120 //for( int i=0;i<16;i++ ){ urg.putc(MD_18001100[i]); }
higedura 0:d2ef19170c3d 121 //for( int i=0;i<16;i++ ){ urg.putc(MD_18001800[i]); }
higedura 0:d2ef19170c3d 122 //for( int i=0;i<16;i++ ){ urg.putc(MD_18001900[i]); }
higedura 0:d2ef19170c3d 123 //for( int i=0;i<16;i++ ){ urg.putc(MD_18001001[i]); }
higedura 0:d2ef19170c3d 124 //for( int i=0;i<16;i++ ){ urg.putc(MD_0FR01001[i]); }
higedura 0:d2ef19170c3d 125 //for( int i=0;i<16;i++ ){ urg.putc(MD_0FR01010[i]); }
higedura 0:d2ef19170c3d 126 //for( int i=0;i<16;i++ ){ urg.putc(MD_09001000[i]); }
higedura 0:d2ef19170c3d 127 //for( int i=0;i<16;i++ ){ urg.putc(MD_09001300[i]); }
higedura 0:d2ef19170c3d 128 //for( int i=0;i<16;i++ ){ urg.putc(MD_09001900[i]); }
higedura 0:d2ef19170c3d 129
higedura 0:d2ef19170c3d 130 //while(1){
higedura 0:d2ef19170c3d 131 while(stop==0){
higedura 0:d2ef19170c3d 132
higedura 0:d2ef19170c3d 133 c_urg_data[2] = c_urg_data[1];
higedura 0:d2ef19170c3d 134 c_urg_data[1] = c_urg_data[0];
higedura 0:d2ef19170c3d 135 c_urg_data[0] = urg.getc();
higedura 0:d2ef19170c3d 136 //fprintf(fp, "%x ",c_urg_data[0]);
higedura 0:d2ef19170c3d 137 //pc.printf("%c",c_urg_data[0]);
higedura 0:d2ef19170c3d 138
higedura 0:d2ef19170c3d 139 if( c_urg_data[2]==a9 && c_urg_data[1]==a9 && c_urg_data[0]==ab ){
higedura 0:d2ef19170c3d 140 //pc.printf("!");
higedura 0:d2ef19170c3d 141 // 7byte: LF + time stamp(4byte) + SUM(1byte) + LF
higedura 0:d2ef19170c3d 142 for( int i=0;i<7;i++ ){ c_urg_data[0] = urg.getc(); }
higedura 0:d2ef19170c3d 143 // data row: 64byte + SUM(1byte) + LF = 66byte
higedura 0:d2ef19170c3d 144 // full range (32rows, last row is 62 byte): i<2110 (2046+32*2*(SUM+LF))
higedura 0:d2ef19170c3d 145 // 90 degrees (13rows, last row is 3 byte): i<797 ( 771+13*2*(SUM+LF))
higedura 0:d2ef19170c3d 146 // 180 degrees (24rows, last row is 3 byte): i<1589 (1539+25*2*(SUM+LF))
higedura 0:d2ef19170c3d 147 for( int i=0;i<N_raw_data;i++ ){ c_urg_data_buf[i] = urg.getc(); }
higedura 0:d2ef19170c3d 148
higedura 0:d2ef19170c3d 149 // i: row - 1, j: cutting last SUM and LF 64(data)+1(SUM)+1(LF)=66, transrating to int from ASCI and -30hex (=-48dec)
higedura 0:d2ef19170c3d 150 // full range: 31
higedura 0:d2ef19170c3d 151 // 90 degrees: 12
higedura 0:d2ef19170c3d 152 // 180 degrees: 24
higedura 0:d2ef19170c3d 153 for( int i=0;i<24;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:d2ef19170c3d 154
higedura 0:d2ef19170c3d 155 // for last row
higedura 0:d2ef19170c3d 156 // * amari totte zidou de data size toreru youni suru (in particular -24)
higedura 0:d2ef19170c3d 157 // full range: j=2046;j<2108 (ignoring last SUM and LF, 2110-2=2108)
higedura 0:d2ef19170c3d 158 // 90 degrees: j=792;j<795 (ignoring last SUM and LF, 797-2=795) i_urg_data_buf[j-24]
higedura 0:d2ef19170c3d 159 // 180 degrees: j=1536;j<1539 (ignoring last SUM and LF, 1589-2=1587) i_urg_data_buf[j-48]
higedura 0:d2ef19170c3d 160 for( int j=1584;j<1587;j++ ){ i_urg_data_buf[j-48] = (int)c_urg_data_buf[j]-48; }
higedura 0:d2ef19170c3d 161
higedura 0:d2ef19170c3d 162 // full range: step 725-43=682
higedura 0:d2ef19170c3d 163 // 90 degrees: step 512-256=257
higedura 0:d2ef19170c3d 164 // 180 degrees: step 640-127=513
higedura 0:d2ef19170c3d 165 for( int i=0;i<N_laser;i++ ){
higedura 0:d2ef19170c3d 166
higedura 0:d2ef19170c3d 167 // decoding 3chara -> depth
higedura 0:d2ef19170c3d 168 for( int j=0;j<3;j++ ){
higedura 0:d2ef19170c3d 169 // binary no toki ha 6-digit -> k<6
higedura 0:d2ef19170c3d 170 for( int k=0;k<6;k++ ){
higedura 0:d2ef19170c3d 171 b_urg_data[2][k] = b_urg_data[1][k];
higedura 0:d2ef19170c3d 172 b_urg_data[1][k] = b_urg_data[0][k];
higedura 0:d2ef19170c3d 173 }
higedura 0:d2ef19170c3d 174 // transforming decimal to binary
higedura 0:d2ef19170c3d 175 for( int k=0;k<6;k++ ){
higedura 0:d2ef19170c3d 176 b_urg_data[0][k] = i_urg_data_buf[3*i+j]%2;
higedura 0:d2ef19170c3d 177 i_urg_data_buf[3*i+j] = i_urg_data_buf[3*i+j]/2;
higedura 0:d2ef19170c3d 178 }
higedura 0:d2ef19170c3d 179 }
higedura 0:d2ef19170c3d 180 // uniting 6-digit binary
higedura 0:d2ef19170c3d 181 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:d2ef19170c3d 182 for( int j=0;j<3;j++ ){ i_urg_depth[i] += b_urg_data_sum[j]; }
higedura 0:d2ef19170c3d 183 urg_depth[i] = (double)i_urg_depth[i]/1000;
higedura 0:d2ef19170c3d 184 for( int j=0;j<3;j++ ){ b_urg_data_sum[j] = 0; }
higedura 0:d2ef19170c3d 185 if( urg_depth[i]<0.02 || range_iaf_max<urg_depth[i] ){ urg_depth[i] = range_iaf_max; } // urg_depth [m]
higedura 0:d2ef19170c3d 186
higedura 0:d2ef19170c3d 187 }
higedura 0:d2ef19170c3d 188
higedura 0:d2ef19170c3d 189 for( int i=0;i<laser_center;i++ ){
higedura 0:d2ef19170c3d 190 iaf_front += urg_depth[i+1+laser_center] * urg_depth[i+laser_center] * sin_iaf_angle / 2; // iaf_front [m^2]
higedura 0:d2ef19170c3d 191 iaf_back += urg_depth[i+1] * urg_depth[i] * sin_iaf_angle / 2;
higedura 0:d2ef19170c3d 192 }
higedura 0:d2ef19170c3d 193
higedura 0:d2ef19170c3d 194 steering_angle_rad = 2 * gain_iaf * (iaf_back - iaf_front) / area_focused; // [rad]
higedura 0:d2ef19170c3d 195
higedura 0:d2ef19170c3d 196 steering_angle_deg = steering_angle_rad*180/pi + 90;
higedura 0:d2ef19170c3d 197
higedura 0:d2ef19170c3d 198 if(steering_angle_rad<0){
higedura 0:d2ef19170c3d 199 steering_angle_rad = 0;
higedura 0:d2ef19170c3d 200 }else if(180<steering_angle_rad){
higedura 0:d2ef19170c3d 201 steering_angle_rad = 180;
higedura 0:d2ef19170c3d 202 }
higedura 0:d2ef19170c3d 203
higedura 0:d2ef19170c3d 204 fprintf(fp, "%8.2f %9.6f %9.6f %6.2f ", t_system.read(), iaf_front, iaf_back, steering_angle_deg-90);
higedura 0:d2ef19170c3d 205 for(int i=0;i<N_laser;i++){ fprintf(fp, "%6.3f ", urg_depth[i]); }
higedura 0:d2ef19170c3d 206 fprintf(fp, "\r\n");
higedura 0:d2ef19170c3d 207
higedura 0:d2ef19170c3d 208 i_steering_angle_send_buf1 = (int)(steering_angle_deg * 100);
higedura 0:d2ef19170c3d 209 i_steering_angle_send_buf2[0] = (int)(i_steering_angle_send_buf1 / 10000);
higedura 0:d2ef19170c3d 210 i_steering_angle_send_buf2[1] = (int)((i_steering_angle_send_buf1 - 10000 * i_steering_angle_send_buf2[0]) / 1000);
higedura 0:d2ef19170c3d 211 i_steering_angle_send_buf2[2] = (int)((i_steering_angle_send_buf1 - 10000 * i_steering_angle_send_buf2[0] - 1000 * i_steering_angle_send_buf2[1]) / 100);
higedura 0:d2ef19170c3d 212 i_steering_angle_send_buf2[3] = (int)((i_steering_angle_send_buf1 - 10000 * i_steering_angle_send_buf2[0] - 1000 * i_steering_angle_send_buf2[1] - 100 * i_steering_angle_send_buf2[2]) / 10);
higedura 0:d2ef19170c3d 213 i_steering_angle_send_buf2[4] = (int) (i_steering_angle_send_buf1 - 10000 * i_steering_angle_send_buf2[0] - 1000 * i_steering_angle_send_buf2[1] - 100 * i_steering_angle_send_buf2[2] - 10 * i_steering_angle_send_buf2[3]);
higedura 0:d2ef19170c3d 214
higedura 0:d2ef19170c3d 215 for(int i=0;i<5;i++){ ascii_steering_angle_send[i] = i_steering_angle_send_buf2[i] + 48; }
higedura 0:d2ef19170c3d 216 for(int i=0;i<5;i++){ yalHack.putc(ascii_steering_angle_send[i]); }
higedura 0:d2ef19170c3d 217
higedura 0:d2ef19170c3d 218 //for(int i=0;i<5;i++){ pc.printf("%f %d %d %d %d %d\r\n", steering_angle_deg, i_steering_angle_send_buf2[0], i_steering_angle_send_buf2[1], i_steering_angle_send_buf2[2], i_steering_angle_send_buf2[3], i_steering_angle_send_buf2[4]); }
higedura 0:d2ef19170c3d 219
higedura 0:d2ef19170c3d 220 }
higedura 0:d2ef19170c3d 221
higedura 0:d2ef19170c3d 222 // Initialization
higedura 0:d2ef19170c3d 223 for( int i=0;i<N_laser;i++ ){ i_urg_depth[i] = 0; }
higedura 0:d2ef19170c3d 224 //for( int i=0;i<N_laser-1;i++ ){ iaf_triangle[i] = 0; }
higedura 0:d2ef19170c3d 225 iaf_front = 0;
higedura 0:d2ef19170c3d 226 iaf_back = 0;
higedura 0:d2ef19170c3d 227
higedura 0:d2ef19170c3d 228
higedura 0:d2ef19170c3d 229 }
higedura 0:d2ef19170c3d 230
higedura 0:d2ef19170c3d 231 t_system.stop();
higedura 0:d2ef19170c3d 232
higedura 0:d2ef19170c3d 233 for( int i=0;i<3;i++ ){ urg.putc(QT[i]); }
higedura 0:d2ef19170c3d 234 fclose(fp);
higedura 0:d2ef19170c3d 235 pc.printf("finish\r\n");
higedura 0:d2ef19170c3d 236 led1 = 0;
higedura 0:d2ef19170c3d 237 led2 = 1;
higedura 0:d2ef19170c3d 238 led3 = 1;
higedura 0:d2ef19170c3d 239 led4 = 0;
higedura 0:d2ef19170c3d 240
higedura 0:d2ef19170c3d 241 }