main.cpp

00001 #include "mbed.h"
00002 #include "servo_api.h"
00003 #include "camera_api.h"
00004 #include "motor_api.h"
00005 
00006 #define Debug_cam_uart
00007 
00008 
00009 Serial pc(USBTX, USBRX);
00010 BX_servo servo; 
00011 BX_camera cam;
00012 BX_motor MotorA('A');
00013 BX_motor MotorB('B');
00014 
00015 //PID cam_to_M_ctrlr(10.0, 118.0, 0.06, 0.11, (0.104/30), 0.0, 0.0, 10);
00016 
00017 
00018 int main() {
00019   
00020     double left = 0.025, middle = 0.038, right = 0.057;
00021     double error, turn, last_turn = middle, avg = middle;
00022     double last_error = 0;
00023     double Kp = 0.000327, Ki = 0.0001;
00024     int black_centerR, black_centerL, center, times = 0;
00025     int flag = 0, sp = 64;
00026     
00027     char psudo_line[128];
00028     
00029 #ifdef Debug_cam_uart
00030      pc.baud(115200);
00031      
00032      
00033   while(1){   
00034         
00035         cam.read();
00036 
00037         MotorA.rotate(0.0);
00038         MotorB.rotate(0.0);
00039         
00040         black_centerL = cam.black_centerL();
00041         black_centerR = cam.black_centerR();
00042         
00043         //center = (black_centerL + black_centerR) / 2;
00044         
00045         /*while(1){
00046             for(double i = 0.025; i < 0.058; i+= 0.001){
00047                 servo.set_angle(0.038);
00048                 wait_ms(1000);
00049             
00050             }
00051         }*/
00052 
00053         MotorA.rotate(0.15);
00054         MotorB.rotate(0.15);
00055         
00056         //catch little error
00057         if(black_centerL > 64 && black_centerR > 64){
00058 
00059             center = (black_centerL <= black_centerR) ? black_centerL : black_centerR;
00060 
00061         } else if(black_centerL > 64 && black_centerR < 64){
00062             
00063             center = (black_centerL-64 <= 64-black_centerR) ? black_centerL : black_centerR;
00064 
00065 
00066         } else if(black_centerL < 64 && black_centerR > 64){
00067 
00068             center = (64-black_centerL <= black_centerR-64) ? black_centerL : black_centerR;
00069 
00070         } else if(black_centerL < 64 && black_centerR < 64){
00071 
00072             center = (black_centerL >= black_centerR) ? black_centerL : black_centerR;
00073 
00074         } else{
00075 
00076             center = 64;
00077         }
00078 
00079 
00080 
00081         if(black_centerL == 118 && black_centerR == 10){//no line flag = 0
00082             
00083             error = 0;
00084             turn = avg;
00085 
00086             flag = 0;
00087             
00088         } else if (black_centerL == 118 && black_centerR != 10){//no left line turn right flag = 1
00089             
00090             sp = 30;
00091             
00092             if(flag == 2){
00093 
00094                 sp = 64;
00095 
00096             }
00097 
00098             error = sp - black_centerR;
00099             turn = middle + Kp * error;
00100 
00101             flag = 1;
00102             last_error = (1/3)*last_error + error;
00103         
00104         } else if (black_centerL != 118 && black_centerR == 10){//no right line turn left flag = 2
00105             
00106             sp = 90;
00107 
00108             if(flag == 1){
00109 
00110                 sp = 64;
00111 
00112             }
00113 
00114             error = 90 - black_centerL;
00115             turn = middle + Kp * error;
00116             
00117             flag = 2;
00118             last_error = (1/3)*last_error + error;
00119             
00120         } else {//flag = 3
00121 
00122             if(60 < center && center < 68){
00123                 
00124                 turn = middle;
00125                 
00126             } else{
00127                 error = 64 - center;
00128                 turn = Kp * error + middle;
00129             }
00130 
00131             if(flag == 1){
00132 
00133                 if(black_centerL < 100){
00134                     servo.set_angle(avg-0.004);
00135                     turn = avg;
00136                 }
00137 
00138             } else if(flag == 2){
00139 
00140                 if(black_centerR > 28){
00141                     servo.set_angle(avg+0.004);
00142                     turn = avg;
00143                 }
00144                 
00145             }
00146             flag = 3;
00147         }
00148 
00149         last_turn += turn;
00150         times++;
00151 
00152         if(times == 2){
00153             
00154             avg = last_turn / times;
00155             servo.set_angle(avg);
00156             times = last_turn = 0;
00157             
00158         }
00159 
00160        
00161         for(int i = 128; i > 64;i--){
00162             if(i==64)
00163                 pc.printf("X");
00164             else          
00165                 pc.printf("%c", cam.sign_line_imageL[i]);
00166             }
00167             
00168             pc.printf("           ||             ");
00169 
00170             for(int i = 64; i > 0; i--){
00171                 if(i==64)
00172                     pc.printf("X");
00173                 else          
00174                     pc.printf("%c", cam.sign_line_imageR[i]);
00175             }
00176         pc.printf("\r\n");
00177         pc.printf("black centerL: %d   black_centerR: %d   psudo_line: %d avg: %lf flag: %d \r\n", black_centerL, black_centerR, center, avg, flag);
00178         
00179         
00180         
00181          //output the psudo map
00182             /*for(int i = 127; i >= 0; i--)   
00183                 psudo_line[i] = '0';
00184             
00185             for(int i = center-5; i < center+5; i++)
00186                 psudo_line[i] = ' ';
00187             
00188             for(int i = 117; i > 10; i--){
00189                 pc.printf("%c", psudo_line[i]);    
00190             }*/ 
00191         
00192         
00193         
00194        
00195        }
00196 
00197         
00198      
00199      
00200      
00201      
00202      
00203      
00204         //   pc.printf("ang :%d\r\n ",( (64.0-center) /64.0  )*90);
00205    //--------------------------------------------            
00206 
00207 
00208          // servo.set_angle(( (64.0-center) /64.0  )*90 );
00209 
00210        
00211          
00212          
00213      
00214      
00215   #endif   
00216        
00217      
00218      
00219      
00220      
00221      
00222      
00223      
00224      
00225      
00226      
00227      
00228     
00229     
00230     
00231     
00232     return 0;
00233     
00234     
00235 }