hige dura
This code is Information Amount Feedback (IAF) with HOKUYO URG.
Revision 0:d2ef19170c3d, committed 2015-01-16
- Comitter:
- higedura
- Date:
- Fri Jan 16 09:51:32 2015 +0000
- Commit message:
- Information amount feedback with URG
Changed in this revision
diff -r 000000000000 -r d2ef19170c3d ASCII.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ASCII.h Fri Jan 16 09:51:32 2015 +0000 @@ -0,0 +1,68 @@ +#define aPeriod 0x2e +#define aLF 0x0a +#define aSpace 0x20 +#define aHash 0x23 +#define aZero 0x30 +#define a1 0x31 +#define a2 0x32 +#define a3 0x33 +#define a4 0x34 +#define a5 0x35 +#define a6 0x36 +#define a7 0x37 +#define a8 0x38 +#define a9 0x39 +#define aColon 0x3a +#define aSemicolon 0x3b +#define aA 0x41 +#define aB 0x42 +#define aC 0x43 +#define aD 0x44 +#define aE 0x45 +#define aF 0x46 +#define aG 0x47 +#define aH 0x48 +#define aI 0x49 +#define aJ 0x4a +#define aK 0x4b +#define aL 0x4c +#define aM 0x4d +#define aN 0x4e +#define aO 0x4f +#define aP 0x50 +#define aQ 0x51 +#define aR 0x52 +#define aS 0x53 +#define aT 0x54 +#define aU 0x55 +#define aV 0x56 +#define aW 0x57 +#define aX 0x58 +#define aY 0x59 +#define aZ 0x5a +#define aa 0x61 +#define ab 0x62 +#define ac 0x63 +#define ad 0x64 +#define ae 0x65 +#define af 0x66 +#define ag 0x67 +#define ah 0x68 +#define ai 0x69 +#define aj 0x6a +#define ak 0x6b +#define al 0x6c +#define am 0x6d +#define an 0x6e +#define ao 0x6f +#define ap 0x70 +#define aq 0x71 +#define ar 0x72 +#define as 0x73 +#define at 0x74 +#define au 0x75 +#define av 0x76 +#define aw 0x77 +#define ax 0x78 +#define ay 0x79 +#define az 0x7a \ No newline at end of file
diff -r 000000000000 -r d2ef19170c3d URG.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/URG.h Fri Jan 16 09:51:32 2015 +0000
@@ -0,0 +1,31 @@
+#include "ASCII.h"
+
+int SCIP2 [8] = {aS, aC, aI, aP, a2, aPeriod, aZero, aLF};
+
+// kannsei site nakute gomennasai
+// | MD | start step (4byte) | stop step (4byte) | N of assembled steps (2byte) | N of thinned scans (1byte) | N of transmissions (2byte) | LF (1byte) |
+// If N of transmissions is 00, URG senses continuously. 0FR means Full Range, and 090 means that range is 90 degrees.
+int MD_0FR01000[16] = {aM, aD, aZero, aZero, a4, a4, aZero, a7, a2, a5, aZero, a1, aZero, aZero, aZero, aLF};
+int MD_0FR01001[16] = {aM, aD, aZero, aZero, a4, a4, aZero, a7, a2, a5, aZero, a1, aZero, aZero, a1, aLF};
+int MD_0FR01010[16] = {aM, aD, aZero, aZero, a4, a4, aZero, a7, a2, a5, aZero, a1, aZero, a1, aZero, aLF};
+int MD_09001000[16] = {aM, aD, aZero, a2, a5, a6, aZero, a5, a1, a2, aZero, a1, aZero, aZero, aZero, aLF};
+int MD_09001300[16] = {aM, aD, aZero, a2, a5, a6, aZero, a5, a1, a2, aZero, a1, a3, aZero, aZero, aLF};
+int MD_09001900[16] = {aM, aD, aZero, a2, a5, a6, aZero, a5, a1, a2, aZero, a1, a9, aZero, aZero, aLF};
+int MD_18001000[16] = {aM, aD, aZero, a1, a2, a8, aZero, a6, a4, aZero, aZero, a1, aZero, aZero, aZero, aLF};
+int MD_18001100[16] = {aM, aD, aZero, a1, a2, a8, aZero, a6, a4, aZero, aZero, a1, a1, aZero, aZero, aLF};
+int MD_18001800[16] = {aM, aD, aZero, a1, a2, a8, aZero, a6, a4, aZero, aZero, a1, a8, aZero, aZero, aLF};
+int MD_18001900[16] = {aM, aD, aZero, a1, a2, a8, aZero, a6, a4, aZero, aZero, a1, a9, aZero, aZero, aLF};
+int MD_18001001[16] = {aM, aD, aZero, a1, a2, a8, aZero, a6, a4, aZero, aZero, a1, aZero, aZero, a1, aLF};
+
+int BM [3] = {aB, aM, aLF};
+int QT [3] = {aQ, aT, aLF};
+int RS [3] = {aR, aS, aLF};
+int RT [3] = {aR, aT, aLF};
+int TMstart [4] = {aT, aM, aZero, aLF};
+int TMrefer [4] = {aT, aM, a1, aLF};
+int TMstop [4] = {aT, aM, a2, aLF};
+int SS576 [9] = {aS, aS, aZero, a5, a7, a6, aZero, aZero, aLF};
+int SS1152 [9] = {aS, aS, a1, a1, a5, a2, aZero, aZero, aLF};
+int VV [3] = {aV, aV, aLF};
+int PP [3] = {aP, aP, aLF};
+int II [3] = {aI, aI, aLF};
diff -r 000000000000 -r d2ef19170c3d main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Jan 16 09:51:32 2015 +0000
@@ -0,0 +1,241 @@
+#include "mbed.h"
+#include "ASCII.h"
+#include "URG.h"
+
+#define pi 3.14159265
+/*
+// full range
+#define N_raw_data 2110
+#define N_usable_data 2046
+#define N_laser 682
+*/
+/*
+// 90 deg
+#define N_raw_data 797
+#define N_usable_data 771
+#define N_laser 257
+*/
+// 180 deg
+#define N_raw_data 1589
+#define N_usable_data 1539
+#define N_laser 513
+
+Serial pc(USBTX, USBRX);
+LocalFileSystem local("local"); // Create the local filesystem under the name "local"
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+Serial yalHack(p9,p10); // tx, rx
+
+DigitalIn stop(p12);
+
+Serial urg(p28, p27); // tx, rx
+
+Timer t_system;
+
+int main() {
+
+ t_system.start();
+
+ double range_iaf_max = 2; // [mm]
+ double angle_of_view = 180; // [deg]
+ double gain_iaf = 1;
+ double area_focused = range_iaf_max * range_iaf_max * pi * angle_of_view / 360;
+
+ int laser_center = (N_laser-1)/2;
+
+ double steering_angle_rad = 0;
+ double steering_angle_deg = 0;
+ int i_steering_angle_send_buf1 = 0;
+ int i_steering_angle_send_buf2[5] = {0};
+ char ascii_steering_angle_send[5] = {0};
+
+ pc.baud(921600);
+ yalHack.baud(921600);
+ urg.baud(19200);
+
+ char c_urg_data[3] = { 0, aA, 0 };
+ char c_urg_data_buf[N_raw_data] = {0};
+ int i_urg_data_buf[N_usable_data] = {0};
+ int b_urg_data[3][6] = {0};
+ int b_urg_data_sum[3] = {0};
+ int pow2[18] = {0};
+ int i_urg_depth[N_laser] = {0};
+
+ double urg_depth[N_laser] = {0};
+ //double iaf_triangle[N_laser-1] = {0};
+ double iaf_front = 0;
+ double iaf_back = 0;
+ double sin_iaf_angle = sin(0.352*pi/180);
+
+ double i_pow = 0;
+
+ 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
+ pc.printf("\n\r\n\rWaiting start up URG\n\r");
+ //xbee.printf("\n\r\n\rWaiting start up URG\n\r");
+ led1 = 1; led2 = 1; led3 = 1; led4 = 1;
+ wait(1); led4 = 0;
+ wait(1); led3 = 0;
+ wait(1); led2 = 0;
+ wait(1); led1 = 0;
+
+ // SCIP 1.0 --> SCIP 2.0
+ pc.printf("SCIP 2.0\n\r");
+ //xbee.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(.25); led4 = 0;
+ wait(.25); led3 = 0;
+ wait(.25); led2 = 0;
+ wait(.25); led1 = 0;
+
+ // Changing baudrate
+ pc.printf("Changing baudrate\n\r");
+ //xbee.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(.25); led4 = 0;
+ wait(.25); led3 = 0;
+ wait(.25); led2 = 0;
+ wait(.25); led1 = 0;
+
+ // Getting data
+ // MD | range (3-digit deg) | N of assembled steps (2-digit) | N of thinned scans (1-digit) | N of transmissions (2-digit) |
+ // 0FR means full range (= 239.4 degs).
+ // i.e.) MD_09001301 -> range = 90 degs, scanning full steps, scanning every 3rd, transmitting once
+ led1 = 1; led4 = 1;
+ for( int i=0;i<16;i++ ){ urg.putc(MD_18001000[i]); }
+ //for( int i=0;i<16;i++ ){ urg.putc(MD_18001100[i]); }
+ //for( int i=0;i<16;i++ ){ urg.putc(MD_18001800[i]); }
+ //for( int i=0;i<16;i++ ){ urg.putc(MD_18001900[i]); }
+ //for( int i=0;i<16;i++ ){ urg.putc(MD_18001001[i]); }
+ //for( int i=0;i<16;i++ ){ urg.putc(MD_0FR01001[i]); }
+ //for( int i=0;i<16;i++ ){ urg.putc(MD_0FR01010[i]); }
+ //for( int i=0;i<16;i++ ){ urg.putc(MD_09001000[i]); }
+ //for( int i=0;i<16;i++ ){ urg.putc(MD_09001300[i]); }
+ //for( int i=0;i<16;i++ ){ urg.putc(MD_09001900[i]); }
+
+ //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();
+ //fprintf(fp, "%x ",c_urg_data[0]);
+ //pc.printf("%c",c_urg_data[0]);
+
+ if( c_urg_data[2]==a9 && c_urg_data[1]==a9 && c_urg_data[0]==ab ){
+ //pc.printf("!");
+ // 7byte: LF + time stamp(4byte) + SUM(1byte) + LF
+ for( int i=0;i<7;i++ ){ c_urg_data[0] = urg.getc(); }
+ // data row: 64byte + SUM(1byte) + LF = 66byte
+ // full range (32rows, last row is 62 byte): i<2110 (2046+32*2*(SUM+LF))
+ // 90 degrees (13rows, last row is 3 byte): i<797 ( 771+13*2*(SUM+LF))
+ // 180 degrees (24rows, last row is 3 byte): i<1589 (1539+25*2*(SUM+LF))
+ for( int i=0;i<N_raw_data;i++ ){ c_urg_data_buf[i] = urg.getc(); }
+
+ // i: row - 1, j: cutting last SUM and LF 64(data)+1(SUM)+1(LF)=66, transrating to int from ASCI and -30hex (=-48dec)
+ // full range: 31
+ // 90 degrees: 12
+ // 180 degrees: 24
+ 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; } }
+
+ // for last row
+ // * amari totte zidou de data size toreru youni suru (in particular -24)
+ // full range: j=2046;j<2108 (ignoring last SUM and LF, 2110-2=2108)
+ // 90 degrees: j=792;j<795 (ignoring last SUM and LF, 797-2=795) i_urg_data_buf[j-24]
+ // 180 degrees: j=1536;j<1539 (ignoring last SUM and LF, 1589-2=1587) i_urg_data_buf[j-48]
+ for( int j=1584;j<1587;j++ ){ i_urg_data_buf[j-48] = (int)c_urg_data_buf[j]-48; }
+
+ // full range: step 725-43=682
+ // 90 degrees: step 512-256=257
+ // 180 degrees: step 640-127=513
+ for( int i=0;i<N_laser;i++ ){
+
+ // decoding 3chara -> depth
+ for( int j=0;j<3;j++ ){
+ // binary no toki ha 6-digit -> k<6
+ 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];
+ }
+ // transforming decimal to binary
+ 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;
+ }
+ }
+ // uniting 6-digit binary
+ 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++ ){ i_urg_depth[i] += b_urg_data_sum[j]; }
+ urg_depth[i] = (double)i_urg_depth[i]/1000;
+ for( int j=0;j<3;j++ ){ b_urg_data_sum[j] = 0; }
+ if( urg_depth[i]<0.02 || range_iaf_max<urg_depth[i] ){ urg_depth[i] = range_iaf_max; } // urg_depth [m]
+
+ }
+
+ for( int i=0;i<laser_center;i++ ){
+ iaf_front += urg_depth[i+1+laser_center] * urg_depth[i+laser_center] * sin_iaf_angle / 2; // iaf_front [m^2]
+ iaf_back += urg_depth[i+1] * urg_depth[i] * sin_iaf_angle / 2;
+ }
+
+ steering_angle_rad = 2 * gain_iaf * (iaf_back - iaf_front) / area_focused; // [rad]
+
+ steering_angle_deg = steering_angle_rad*180/pi + 90;
+
+ if(steering_angle_rad<0){
+ steering_angle_rad = 0;
+ }else if(180<steering_angle_rad){
+ steering_angle_rad = 180;
+ }
+
+ fprintf(fp, "%8.2f %9.6f %9.6f %6.2f ", t_system.read(), iaf_front, iaf_back, steering_angle_deg-90);
+ for(int i=0;i<N_laser;i++){ fprintf(fp, "%6.3f ", urg_depth[i]); }
+ fprintf(fp, "\r\n");
+
+ i_steering_angle_send_buf1 = (int)(steering_angle_deg * 100);
+ i_steering_angle_send_buf2[0] = (int)(i_steering_angle_send_buf1 / 10000);
+ i_steering_angle_send_buf2[1] = (int)((i_steering_angle_send_buf1 - 10000 * i_steering_angle_send_buf2[0]) / 1000);
+ 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);
+ 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);
+ 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]);
+
+ for(int i=0;i<5;i++){ ascii_steering_angle_send[i] = i_steering_angle_send_buf2[i] + 48; }
+ for(int i=0;i<5;i++){ yalHack.putc(ascii_steering_angle_send[i]); }
+
+ //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]); }
+
+ }
+
+ // Initialization
+ for( int i=0;i<N_laser;i++ ){ i_urg_depth[i] = 0; }
+ //for( int i=0;i<N_laser-1;i++ ){ iaf_triangle[i] = 0; }
+ iaf_front = 0;
+ iaf_back = 0;
+
+
+ }
+
+ t_system.stop();
+
+ for( int i=0;i<3;i++ ){ urg.putc(QT[i]); }
+ fclose(fp);
+ pc.printf("finish\r\n");
+ led1 = 0;
+ led2 = 1;
+ led3 = 1;
+ led4 = 0;
+
+}
diff -r 000000000000 -r d2ef19170c3d mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Jan 16 09:51:32 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/031413cf7a89 \ No newline at end of file