hige dura
This code is Information Amount Feedback (IAF) with HOKUYO URG.
Diff: main.cpp
- Revision:
- 0:d2ef19170c3d
--- /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;
+
+}