hige dura / Mbed 2 deprecated dotHR_URG

dotHR_URG

Dependencies:   FatFileSystem TextLCD mbed

main.cpp@0:a1accf06b614, 2012-11-30 (annotated)

Committer:
higedura
Date:
Fri Nov 30 12:34:22 2012 +0000
Revision:
0:a1accf06b614
Child:
1:52d3136e1a22
dotHR_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 0:a1accf06b614 14 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
higedura 0:a1accf06b614 15 InterruptIn urg_flag(p12);
higedura 0:a1accf06b614 16 Serial navi(p13, p14); // tx, rx
higedura 0:a1accf06b614 17 DigitalIn stop(p17); // button (green)
higedura 0:a1accf06b614 18 Serial urg(p28, p27); // tx, rx
higedura 0:a1accf06b614 19 //TextLCD lcd(p24, p26, p27, p28, p29, p30); // rs, e, d4-d7
higedura 0:a1accf06b614 20
higedura 0:a1accf06b614 21 void send_to_navi();
higedura 0:a1accf06b614 22
higedura 0:a1accf06b614 23 int iaf_delta = 500;
higedura 0:a1accf06b614 24 double iaf_SR_print = 0;
higedura 0:a1accf06b614 25 double iaf_SL_print = 0;
higedura 0:a1accf06b614 26
higedura 0:a1accf06b614 27 int t = -1;
higedura 0:a1accf06b614 28 int dt = 1;
higedura 0:a1accf06b614 29
higedura 0:a1accf06b614 30 int main() {
higedura 0:a1accf06b614 31
higedura 0:a1accf06b614 32 //int t = -2;
higedura 0:a1accf06b614 33 //int dt = 1;
higedura 0:a1accf06b614 34
higedura 0:a1accf06b614 35 int flag = 0;
higedura 0:a1accf06b614 36
higedura 0:a1accf06b614 37 char c_urg_data[3] = { 0, aA, 0 };
higedura 0:a1accf06b614 38 char c_urg_data_buf[2110] = {0};
higedura 0:a1accf06b614 39 int i_urg_data_buf[2046] = {0};
higedura 0:a1accf06b614 40 int b_urg_data[3][6] = {0};
higedura 0:a1accf06b614 41 int b_urg_data_sum[3] = {0};
higedura 0:a1accf06b614 42 int pow2[18] = {0};
higedura 0:a1accf06b614 43 int urg_depth[682] = {0};
higedura 0:a1accf06b614 44
higedura 0:a1accf06b614 45 int depth_sum_right = 0;
higedura 0:a1accf06b614 46 int depth_sum_left = 0;
higedura 0:a1accf06b614 47
higedura 0:a1accf06b614 48 // for IAF +-45deg 1600mm
higedura 0:a1accf06b614 49 double range = 1600;
higedura 0:a1accf06b614 50 double sin_iaf_angle = sin(0.352*pi/180); // mabiki nashi
higedura 0:a1accf06b614 51 double iaf_SE = (range/1000)*(range/1000)*pi/4;
higedura 0:a1accf06b614 52 double iaf_IL = 0;
higedura 0:a1accf06b614 53 double iaf_gain = 1000;
higedura 0:a1accf06b614 54 //int iaf_delta = 0;
higedura 0:a1accf06b614 55 double iaf_SR = 0;
higedura 0:a1accf06b614 56 double iaf_SL = 0;
higedura 0:a1accf06b614 57
higedura 0:a1accf06b614 58 // Information Requirement 0<IR<1
higedura 0:a1accf06b614 59 double iaf_IR = 0.9;
higedura 0:a1accf06b614 60
higedura 0:a1accf06b614 61 double i_pow = 0;
higedura 0:a1accf06b614 62
higedura 0:a1accf06b614 63 pc.baud(921600);
higedura 0:a1accf06b614 64 navi.baud(115200);
higedura 0:a1accf06b614 65 urg.baud(19200);
higedura 0:a1accf06b614 66
higedura 0:a1accf06b614 67 for( int i=0;i<18;i++ ){
higedura 0:a1accf06b614 68 i_pow=(double)i;
higedura 0:a1accf06b614 69 pow2[i] = pow(2,i_pow);
higedura 0:a1accf06b614 70 };
higedura 0:a1accf06b614 71
higedura 0:a1accf06b614 72 // Waiting start up URG
higedura 0:a1accf06b614 73 //lcd.cls();
higedura 0:a1accf06b614 74 //lcd.printf("Waiting start up URG");
higedura 0:a1accf06b614 75 pc.printf("\n\r\n\rWaiting start up URG\n\r");
higedura 0:a1accf06b614 76 led1 = 1; led2 = 1; led3 = 1; led4 = 1;
higedura 0:a1accf06b614 77 wait(.5); led4 = 0;
higedura 0:a1accf06b614 78 wait(.5); led3 = 0;
higedura 0:a1accf06b614 79 wait(.5); led2 = 0;
higedura 0:a1accf06b614 80 wait(.5); led1 = 0;
higedura 0:a1accf06b614 81
higedura 0:a1accf06b614 82 // SCIP 1.0 --> SCIP 2.0
higedura 0:a1accf06b614 83 //lcd.cls();
higedura 0:a1accf06b614 84 //lcd.printf("SCIP 2.0");
higedura 0:a1accf06b614 85 pc.printf("SCIP 2.0\n\r");
higedura 0:a1accf06b614 86 for( int i=0;i<8;i++ ){ urg.putc(SCIP2[i]); }
higedura 0:a1accf06b614 87 // Waiting SCIP 2.0
higedura 0:a1accf06b614 88 led1 = 1; led2 = 1; led3 = 1; led4 = 1;
higedura 0:a1accf06b614 89 wait(.5); led4 = 0;
higedura 0:a1accf06b614 90 wait(.5); led3 = 0;
higedura 0:a1accf06b614 91 wait(.5); led2 = 0;
higedura 0:a1accf06b614 92 wait(.5); led1 = 0;
higedura 0:a1accf06b614 93
higedura 0:a1accf06b614 94 // Changing baudrate
higedura 0:a1accf06b614 95 //lcd.cls();
higedura 0:a1accf06b614 96 //lcd.printf("Changing baudrate");
higedura 0:a1accf06b614 97 pc.printf("Changing baudrate\n\r");
higedura 0:a1accf06b614 98 for( int i=0;i<9;i++ ){ urg.putc(SS1152[i]); }
higedura 0:a1accf06b614 99 urg.baud(115200);
higedura 0:a1accf06b614 100 led1 = 1; led2 = 1; led3 = 1; led4 = 1;
higedura 0:a1accf06b614 101 wait(.5); led4 = 0;
higedura 0:a1accf06b614 102 wait(.5); led3 = 0;
higedura 0:a1accf06b614 103 wait(.5); led2 = 0;
higedura 0:a1accf06b614 104 wait(.5); led1 = 0;
higedura 0:a1accf06b614 105
higedura 0:a1accf06b614 106 // Getting data
higedura 0:a1accf06b614 107 led1 = 1; led4 = 1;
higedura 0:a1accf06b614 108 //lcd.cls();
higedura 0:a1accf06b614 109 //lcd.printf("Getting data ...");
higedura 0:a1accf06b614 110 pc.printf("Getting data ...\n\r\n\r");
higedura 0:a1accf06b614 111 for( int i=0;i<16;i++ ){ urg.putc(MD[i]); }
higedura 0:a1accf06b614 112 //for( int i=0;i<16;i++ ){ urg.putc(MD1[i]); }
higedura 0:a1accf06b614 113 //for( int i=0;i<16;i++ ){ urg.putc(MD10[i]); }
higedura 0:a1accf06b614 114
higedura 0:a1accf06b614 115 // Cutting first data
higedura 0:a1accf06b614 116 while( flag==0 ){
higedura 0:a1accf06b614 117 c_urg_data[0] = urg.getc();
higedura 0:a1accf06b614 118 if( c_urg_data[0]==aLF && c_urg_data[1]==aLF ){ flag = 1; }
higedura 0:a1accf06b614 119 for( int i=0;i<2;i++ ){ c_urg_data[i+1] = c_urg_data[i]; }
higedura 0:a1accf06b614 120 }
higedura 0:a1accf06b614 121
higedura 0:a1accf06b614 122 urg_flag.rise(&send_to_navi); // attach the address of the flip function to the rising edge
higedura 0:a1accf06b614 123
higedura 0:a1accf06b614 124 //while(1){
higedura 0:a1accf06b614 125 while( stop==0 ){
higedura 0:a1accf06b614 126
higedura 0:a1accf06b614 127 c_urg_data[2] = c_urg_data[1];
higedura 0:a1accf06b614 128 c_urg_data[1] = c_urg_data[0];
higedura 0:a1accf06b614 129 c_urg_data[0] = urg.getc();
higedura 0:a1accf06b614 130
higedura 0:a1accf06b614 131 if( c_urg_data[2]==a9 && c_urg_data[1]==a9 && c_urg_data[0]==ab ){
higedura 0:a1accf06b614 132
higedura 0:a1accf06b614 133 for( int i=0;i<7;i++ ){ c_urg_data[0] = urg.getc(); }
higedura 0:a1accf06b614 134
higedura 0:a1accf06b614 135 for( int i=0;i<2110;i++ ){ c_urg_data_buf[i] = urg.getc(); }
higedura 0:a1accf06b614 136
higedura 0:a1accf06b614 137 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 138
higedura 0:a1accf06b614 139 for( int j=2046;j<2108;j++ ){ i_urg_data_buf[j-62] = (int)c_urg_data_buf[j]-48; }
higedura 0:a1accf06b614 140 /*
higedura 0:a1accf06b614 141 for( int i=0;i<2110;i++ ){ pc.printf("%c", c_urg_data_buf[i]); }
higedura 0:a1accf06b614 142 pc.printf("\n\r\n\r");
higedura 0:a1accf06b614 143
higedura 0:a1accf06b614 144 for( int i=0;i<2046;i++ ){ pc.printf("%2d ", i_urg_data_buf[i]); }
higedura 0:a1accf06b614 145 pc.printf("\n\r\n\r");
higedura 0:a1accf06b614 146 */
higedura 0:a1accf06b614 147 for( int i=0;i<682;i++ ){
higedura 0:a1accf06b614 148
higedura 0:a1accf06b614 149 for( int j=0;j<3;j++ ){
higedura 0:a1accf06b614 150
higedura 0:a1accf06b614 151 for( int k=0;k<6;k++ ){
higedura 0:a1accf06b614 152 b_urg_data[2][k] = b_urg_data[1][k];
higedura 0:a1accf06b614 153 b_urg_data[1][k] = b_urg_data[0][k];
higedura 0:a1accf06b614 154 }
higedura 0:a1accf06b614 155
higedura 0:a1accf06b614 156 for( int k=0;k<6;k++ ){
higedura 0:a1accf06b614 157 b_urg_data[0][k] = i_urg_data_buf[3*i+j]%2;
higedura 0:a1accf06b614 158 i_urg_data_buf[3*i+j] = i_urg_data_buf[3*i+j]/2;
higedura 0:a1accf06b614 159 }
higedura 0:a1accf06b614 160
higedura 0:a1accf06b614 161 }
higedura 0:a1accf06b614 162 /* binary check
higedura 0:a1accf06b614 163 for( int j=0;j<3;j++ ){
higedura 0:a1accf06b614 164 for( int k=0;k<6;k++ ){
higedura 0:a1accf06b614 165 pc.printf("%d", b_urg_data[2-j][5-k]);
higedura 0:a1accf06b614 166 }
higedura 0:a1accf06b614 167 pc.printf(" ");
higedura 0:a1accf06b614 168 }
higedura 0:a1accf06b614 169 */
higedura 0:a1accf06b614 170 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 171 for( int j=0;j<3;j++ ){ urg_depth[i] += b_urg_data_sum[j]; }
higedura 0:a1accf06b614 172 for( int j=0;j<3;j++ ){ b_urg_data_sum[j] = 0; }
higedura 0:a1accf06b614 173 if( urg_depth[i]>range ){ urg_depth[i] = range; }
higedura 0:a1accf06b614 174 //pc.printf("%d ", urg_depth[i]);
higedura 0:a1accf06b614 175 }
higedura 0:a1accf06b614 176 //pc.printf("\r\n\r\n");
higedura 0:a1accf06b614 177
higedura 0:a1accf06b614 178 /*
higedura 0:a1accf06b614 179 // Full range
higedura 0:a1accf06b614 180 for( int i=0;i<341;i++ ){
higedura 0:a1accf06b614 181 depth_sum_right += urg_depth[i];
higedura 0:a1accf06b614 182 depth_sum_left += urg_depth[i+341];
higedura 0:a1accf06b614 183 }
higedura 0:a1accf06b614 184 //pc.printf("\r\n%d %d\r\n\r\n", depth_sum_right, depth_sum_left);
higedura 0:a1accf06b614 185 // Initialization
higedura 0:a1accf06b614 186 for( int i=0;i<682;i++ ){ urg_depth[i] = 0; }
higedura 0:a1accf06b614 187 depth_sum_right = 0;
higedura 0:a1accf06b614 188 depth_sum_left = 0;
higedura 0:a1accf06b614 189 */
higedura 0:a1accf06b614 190
higedura 0:a1accf06b614 191 // IAF (+-45deg) (/1000000 is length conversion mm to m)
higedura 0:a1accf06b614 192 //for( int i=212;i<469;i++ ){ pc.printf("%d ", urg_depth[i]); }
higedura 0:a1accf06b614 193 //pc.printf("\r\n");
higedura 0:a1accf06b614 194 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 195 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 196 // iaf_delta: (10*delta + 500)[deg/s]
higedura 0:a1accf06b614 197 iaf_IL = (iaf_SR+iaf_SL)/iaf_SE;
higedura 0:a1accf06b614 198 if( iaf_IR-iaf_IL>0 ){ iaf_delta = (iaf_gain*(iaf_SR-iaf_SL))/iaf_SE+500; }
higedura 0:a1accf06b614 199 else{ iaf_delta = 500; }
higedura 0:a1accf06b614 200 if( iaf_delta>800 ){ iaf_delta = 800; }
higedura 0:a1accf06b614 201 if( iaf_delta<200 ){ iaf_delta = 200; }
higedura 0:a1accf06b614 202
higedura 0:a1accf06b614 203 //pc.printf("%6.3f, %6.3f\r\n", iaf_SR, iaf_SL);
higedura 0:a1accf06b614 204 pc.printf("%4d\r\n", iaf_delta);
higedura 0:a1accf06b614 205 /*
higedura 0:a1accf06b614 206 if( urg_flag==1 ){
higedura 0:a1accf06b614 207 navi.printf("%4d",iaf_delta);
higedura 0:a1accf06b614 208 //dotHR.printf("a");
higedura 0:a1accf06b614 209 pc.printf("%4d, %4d\r\n", t, iaf_delta);
higedura 0:a1accf06b614 210 t = t+dt;
higedura 0:a1accf06b614 211 }
higedura 0:a1accf06b614 212 */
higedura 0:a1accf06b614 213 // Initialization
higedura 0:a1accf06b614 214 for( int i=0;i<682;i++ ){ urg_depth[i] = 0; }
higedura 0:a1accf06b614 215 iaf_SR_print = iaf_SR;
higedura 0:a1accf06b614 216 iaf_SL_print = iaf_SL;
higedura 0:a1accf06b614 217 iaf_SR = 0;
higedura 0:a1accf06b614 218 iaf_SL = 0;
higedura 0:a1accf06b614 219
higedura 0:a1accf06b614 220 }
higedura 0:a1accf06b614 221
higedura 0:a1accf06b614 222 }
higedura 0:a1accf06b614 223
higedura 0:a1accf06b614 224 }
higedura 0:a1accf06b614 225
higedura 0:a1accf06b614 226 void send_to_navi() {
higedura 0:a1accf06b614 227
higedura 0:a1accf06b614 228 navi.printf("%4d",iaf_delta);
higedura 0:a1accf06b614 229 pc.printf("%4d, %f, %f, %4d\r\n", t, iaf_SL_print, iaf_SR_print, iaf_delta);
higedura 0:a1accf06b614 230 t = t+dt;
higedura 0:a1accf06b614 231
higedura 0:a1accf06b614 232 }