hige dura
This code is Information Amount Feedback (IAF) with HOKUYO URG.
main.cpp
- Committer:
- higedura
- Date:
- 2015-01-16
- Revision:
- 0:d2ef19170c3d
File content as of revision 0:d2ef19170c3d:
#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;
}