hige dura / Mbed 2 deprecated dotHR_URG

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?

UserRevisionLine numberNew 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 }