hige dura / Mbed 2 deprecated dotHR_URG

dotHR_URG

Dependencies:   FatFileSystem TextLCD mbed

main.cpp@1:52d3136e1a22, 2012-12-05 (annotated)

Committer:
higedura
Date:
Wed Dec 05 13:56:13 2012 +0000
Revision:
1:52d3136e1a22
Parent:
0:a1accf06b614
Child:
2:22b200c374cd
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 Serial navi(p13, p14); // tx, rx
higedura 0:a1accf06b614 16 DigitalIn stop(p17); // 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 0:a1accf06b614 20 void send_to_navi();
higedura 0:a1accf06b614 21
higedura 0:a1accf06b614 22 int iaf_delta = 500;
higedura 0:a1accf06b614 23 double iaf_SR_print = 0;
higedura 0:a1accf06b614 24 double iaf_SL_print = 0;
higedura 0:a1accf06b614 25
higedura 0:a1accf06b614 26 int t = -1;
higedura 0:a1accf06b614 27 int dt = 1;
higedura 0:a1accf06b614 28
higedura 0:a1accf06b614 29 int main() {
higedura 0:a1accf06b614 30
higedura 0:a1accf06b614 31 //int t = -2;
higedura 0:a1accf06b614 32 //int dt = 1;
higedura 0:a1accf06b614 33
higedura 0:a1accf06b614 34 int flag = 0;
higedura 0:a1accf06b614 35
higedura 0:a1accf06b614 36 char c_urg_data[3] = { 0, aA, 0 };
higedura 0:a1accf06b614 37 char c_urg_data_buf[2110] = {0};
higedura 0:a1accf06b614 38 int i_urg_data_buf[2046] = {0};
higedura 0:a1accf06b614 39 int b_urg_data[3][6] = {0};
higedura 0:a1accf06b614 40 int b_urg_data_sum[3] = {0};
higedura 0:a1accf06b614 41 int pow2[18] = {0};
higedura 0:a1accf06b614 42 int urg_depth[682] = {0};
higedura 0:a1accf06b614 43
higedura 0:a1accf06b614 44 // for IAF +-45deg 1600mm
higedura 0:a1accf06b614 45 double range = 1600;
higedura 0:a1accf06b614 46 double sin_iaf_angle = sin(0.352*pi/180); // mabiki nashi
higedura 0:a1accf06b614 47 double iaf_SE = (range/1000)*(range/1000)*pi/4;
higedura 0:a1accf06b614 48 double iaf_IL = 0;
higedura 0:a1accf06b614 49 double iaf_gain = 1000;
higedura 0:a1accf06b614 50 //int iaf_delta = 0;
higedura 0:a1accf06b614 51 double iaf_SR = 0;
higedura 0:a1accf06b614 52 double iaf_SL = 0;
higedura 0:a1accf06b614 53
higedura 0:a1accf06b614 54 // Information Requirement 0<IR<1
higedura 0:a1accf06b614 55 double iaf_IR = 0.9;
higedura 0:a1accf06b614 56
higedura 0:a1accf06b614 57 double i_pow = 0;
higedura 0:a1accf06b614 58
higedura 0:a1accf06b614 59 pc.baud(921600);
higedura 0:a1accf06b614 60 navi.baud(115200);
higedura 0:a1accf06b614 61 urg.baud(19200);
higedura 0:a1accf06b614 62
higedura 0:a1accf06b614 63 for( int i=0;i<18;i++ ){
higedura 0:a1accf06b614 64 i_pow=(double)i;
higedura 0:a1accf06b614 65 pow2[i] = pow(2,i_pow);
higedura 0:a1accf06b614 66 };
higedura 0:a1accf06b614 67
higedura 0:a1accf06b614 68 // Waiting start up URG
higedura 0:a1accf06b614 69 //lcd.cls();
higedura 0:a1accf06b614 70 //lcd.printf("Waiting start up URG");
higedura 0:a1accf06b614 71 pc.printf("\n\r\n\rWaiting start up URG\n\r");
higedura 0:a1accf06b614 72 led1 = 1; led2 = 1; led3 = 1; led4 = 1;
higedura 0:a1accf06b614 73 wait(.5); led4 = 0;
higedura 0:a1accf06b614 74 wait(.5); led3 = 0;
higedura 0:a1accf06b614 75 wait(.5); led2 = 0;
higedura 0:a1accf06b614 76 wait(.5); led1 = 0;
higedura 0:a1accf06b614 77
higedura 0:a1accf06b614 78 // SCIP 1.0 --> SCIP 2.0
higedura 0:a1accf06b614 79 //lcd.cls();
higedura 0:a1accf06b614 80 //lcd.printf("SCIP 2.0");
higedura 0:a1accf06b614 81 pc.printf("SCIP 2.0\n\r");
higedura 0:a1accf06b614 82 for( int i=0;i<8;i++ ){ urg.putc(SCIP2[i]); }
higedura 0:a1accf06b614 83 // Waiting SCIP 2.0
higedura 0:a1accf06b614 84 led1 = 1; led2 = 1; led3 = 1; led4 = 1;
higedura 0:a1accf06b614 85 wait(.5); led4 = 0;
higedura 0:a1accf06b614 86 wait(.5); led3 = 0;
higedura 0:a1accf06b614 87 wait(.5); led2 = 0;
higedura 0:a1accf06b614 88 wait(.5); led1 = 0;
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 0:a1accf06b614 96 led1 = 1; led2 = 1; led3 = 1; led4 = 1;
higedura 0:a1accf06b614 97 wait(.5); led4 = 0;
higedura 0:a1accf06b614 98 wait(.5); led3 = 0;
higedura 0:a1accf06b614 99 wait(.5); led2 = 0;
higedura 0:a1accf06b614 100 wait(.5); led1 = 0;
higedura 0:a1accf06b614 101
higedura 0:a1accf06b614 102 // Getting data
higedura 0:a1accf06b614 103 led1 = 1; led4 = 1;
higedura 0:a1accf06b614 104 //lcd.cls();
higedura 0:a1accf06b614 105 //lcd.printf("Getting data ...");
higedura 0:a1accf06b614 106 pc.printf("Getting data ...\n\r\n\r");
higedura 0:a1accf06b614 107 for( int i=0;i<16;i++ ){ urg.putc(MD[i]); }
higedura 0:a1accf06b614 108 //for( int i=0;i<16;i++ ){ urg.putc(MD1[i]); }
higedura 0:a1accf06b614 109 //for( int i=0;i<16;i++ ){ urg.putc(MD10[i]); }
higedura 0:a1accf06b614 110
higedura 0:a1accf06b614 111 // Cutting first data
higedura 0:a1accf06b614 112 while( flag==0 ){
higedura 0:a1accf06b614 113 c_urg_data[0] = urg.getc();
higedura 0:a1accf06b614 114 if( c_urg_data[0]==aLF && c_urg_data[1]==aLF ){ flag = 1; }
higedura 0:a1accf06b614 115 for( int i=0;i<2;i++ ){ c_urg_data[i+1] = c_urg_data[i]; }
higedura 0:a1accf06b614 116 }
higedura 1:52d3136e1a22 117 /*
higedura 0:a1accf06b614 118 //while(1){
higedura 0:a1accf06b614 119 while( stop==0 ){
higedura 0:a1accf06b614 120
higedura 0:a1accf06b614 121 c_urg_data[2] = c_urg_data[1];
higedura 0:a1accf06b614 122 c_urg_data[1] = c_urg_data[0];
higedura 0:a1accf06b614 123 c_urg_data[0] = urg.getc();
higedura 0:a1accf06b614 124
higedura 0:a1accf06b614 125 if( c_urg_data[2]==a9 && c_urg_data[1]==a9 && c_urg_data[0]==ab ){
higedura 0:a1accf06b614 126
higedura 0:a1accf06b614 127 for( int i=0;i<7;i++ ){ c_urg_data[0] = urg.getc(); }
higedura 0:a1accf06b614 128
higedura 0:a1accf06b614 129 for( int i=0;i<2110;i++ ){ c_urg_data_buf[i] = urg.getc(); }
higedura 0:a1accf06b614 130
higedura 0:a1accf06b614 131 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 132
higedura 0:a1accf06b614 133 for( int j=2046;j<2108;j++ ){ i_urg_data_buf[j-62] = (int)c_urg_data_buf[j]-48; }
higedura 1:52d3136e1a22 134
higedura 0:a1accf06b614 135 for( int i=0;i<682;i++ ){
higedura 0:a1accf06b614 136
higedura 0:a1accf06b614 137 for( int j=0;j<3;j++ ){
higedura 0:a1accf06b614 138
higedura 0:a1accf06b614 139 for( int k=0;k<6;k++ ){
higedura 0:a1accf06b614 140 b_urg_data[2][k] = b_urg_data[1][k];
higedura 0:a1accf06b614 141 b_urg_data[1][k] = b_urg_data[0][k];
higedura 0:a1accf06b614 142 }
higedura 0:a1accf06b614 143
higedura 0:a1accf06b614 144 for( int k=0;k<6;k++ ){
higedura 0:a1accf06b614 145 b_urg_data[0][k] = i_urg_data_buf[3*i+j]%2;
higedura 0:a1accf06b614 146 i_urg_data_buf[3*i+j] = i_urg_data_buf[3*i+j]/2;
higedura 0:a1accf06b614 147 }
higedura 0:a1accf06b614 148
higedura 0:a1accf06b614 149 }
higedura 1:52d3136e1a22 150
higedura 0:a1accf06b614 151 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 152 for( int j=0;j<3;j++ ){ urg_depth[i] += b_urg_data_sum[j]; }
higedura 0:a1accf06b614 153 for( int j=0;j<3;j++ ){ b_urg_data_sum[j] = 0; }
higedura 0:a1accf06b614 154 if( urg_depth[i]>range ){ urg_depth[i] = range; }
higedura 1:52d3136e1a22 155
higedura 0:a1accf06b614 156 }
higedura 1:52d3136e1a22 157
higedura 0:a1accf06b614 158 // IAF (+-45deg) (/1000000 is length conversion mm to m)
higedura 1:52d3136e1a22 159
higedura 0:a1accf06b614 160 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 161 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 162 // iaf_delta: (10*delta + 500)[deg/s]
higedura 0:a1accf06b614 163 iaf_IL = (iaf_SR+iaf_SL)/iaf_SE;
higedura 0:a1accf06b614 164 if( iaf_IR-iaf_IL>0 ){ iaf_delta = (iaf_gain*(iaf_SR-iaf_SL))/iaf_SE+500; }
higedura 0:a1accf06b614 165 else{ iaf_delta = 500; }
higedura 0:a1accf06b614 166 if( iaf_delta>800 ){ iaf_delta = 800; }
higedura 0:a1accf06b614 167 if( iaf_delta<200 ){ iaf_delta = 200; }
higedura 1:52d3136e1a22 168
higedura 1:52d3136e1a22 169
higedura 0:a1accf06b614 170 // Initialization
higedura 0:a1accf06b614 171 for( int i=0;i<682;i++ ){ urg_depth[i] = 0; }
higedura 0:a1accf06b614 172 iaf_SR_print = iaf_SR;
higedura 0:a1accf06b614 173 iaf_SL_print = iaf_SL;
higedura 0:a1accf06b614 174 iaf_SR = 0;
higedura 0:a1accf06b614 175 iaf_SL = 0;
higedura 0:a1accf06b614 176
higedura 0:a1accf06b614 177 }
higedura 0:a1accf06b614 178
higedura 0:a1accf06b614 179 }
higedura 1:52d3136e1a22 180 */
higedura 0:a1accf06b614 181 }