main.cpp

00001 #include "mbed.h"
00002 #include "controller.h"
00003 #include "servo_api.h"
00004 #include "camera_api.h"
00005 #include "motor_api.h"
00006 #include "pot.h"
00007 
00008 
00009 #define Debug_cam_uart
00010 #define R_eye
00011 #define motor_on
00012 #define Pcontroller
00013 #define task_ma_time
00014 #define offset 65
00015 #include "TSISensor.h"
00016 
00017 Serial pc(USBTX, USBRX);
00018 BX_servo servo;
00019 BX_camera cam(0);
00020 BX_motor MotorA('A');
00021 BX_motor MotorB('B');
00022 BX_pot pot1('1');            // 90/30=3
00023 BX_pot pot2('2');
00024 PID cam_to_M_ctrlr(10.0,118.0,0.06,0.11,(0.104/30),0.0,0.0,10);
00025 DigitalOut task_pin(PTD1);
00026 TSISensor tsi;
00027 
00028 void run();
00029 
00030 int main()
00031 {
00032     pc.baud(115200);
00033 
00034     /*while(1) {
00035         if(tsi.readPercentage()>0.00011)
00036             break;
00037     }*/
00038     run();
00039 
00040     return 0;
00041 }
00042 
00043 
00044 
00045 void run()
00046 {
00047     double motor, angle=0.0;
00048     double b_r_c;
00049     double PID_v;
00050     double error , P, I, D, kp, r_kp;
00051     double last_error=0.0 ,last_I=0.0, last_brc, brc_df;
00052     bool first_time=true, r_kp_neg=false;
00053 
00054     while(1) {
00055 #ifdef task_ma_time
00056         task_pin=1;
00057 #endif
00058 
00059         cam.read();
00060 #ifdef Debug_cam_uart
00061 #ifdef L_eye
00062         for(int i=0; i<128; i++) {
00063             if(i==64)
00064                 pc.printf("X");
00065             else
00066                 pc.printf("%c", cam.sign_line_imageL[i]);
00067         }
00068         pc.printf("           ||             ");
00069 #endif
00070 #ifdef R_eye
00071         for(int i=128; i>=0; i--) {
00072             if(i==64)
00073                 pc.printf("X");
00074             else if(i<10)
00075                 pc.printf("-");
00076             else if(i>117)
00077                 pc.printf("-");
00078             else
00079                 pc.printf("%c", cam.sign_line_imageR[i]);
00080         }
00081         pc.printf("    ");
00082         //pc.printf("\r\n");
00083 #endif
00084 #endif
00085 
00086 #ifdef motor_on
00087         motor=pot1.read();
00088         MotorA.rotate(motor);
00089         MotorB.rotate(motor);
00090         //pc.printf("%f\r\n",motor);
00091 #endif
00092 
00093         b_r_c=(double)cam.black_centerR();
00094         //pc.printf("%d %d speed :%f  bk_center %f PID:%f \r\n",cam.de_v,cam.de_v2,motor,b_r_c,PID_v);
00095         if(b_r_c<0.0){
00096             pc.printf("\r\n");
00097             continue;
00098             }
00099         //compute
00100         //stand at 65
00101         error=b_r_c-offset;
00102         brc_df=b_r_c-last_brc;
00103         
00104         
00105         if(first_time==true){
00106                 r_kp=0;
00107                 first_time=false;
00108             }
00109         else{
00110                 r_kp+=brc_df;
00111                 if(b_r_c==last_brc)
00112                     r_kp=0;
00113             }
00114         last_brc=b_r_c;
00115             
00116         if(r_kp<0.0){
00117                 r_kp*=-1.0;
00118                 r_kp_neg=true;
00119             }
00120         kp=0.016/(50-(r_kp/10.0));
00121         if(r_kp_neg==true){
00122                 r_kp_neg=false;
00123                 r_kp*=-1.0;
00124             } 
00125                 
00126         P=kp*error;
00127 
00128         I=last_I*2.0/3.0+error;
00129         last_I=I;
00130         I=(kp*0.02/0.04)*I;
00131 
00132         D=error-last_error;
00133         last_error=error;
00134         D=(kp*0.04/0.02)*D;
00135 
00136         PID_v=P;
00137 
00138         if(PID_v < 0.0) {
00139             PID_v*=-1.0;
00140             PID_v=0.077+PID_v;
00141         } else if(PID_v > 0.0)
00142             PID_v=0.077-PID_v;
00143         else
00144             PID_v=0.077;
00145         pc.printf("%lf %lf %lf\r\n",PID_v, r_kp, b_r_c);
00146         //pc.printf("%lf\r\n",angle);
00147         servo.set_angle(PID_v);
00148 
00149 #ifdef task_ma_time
00150         task_pin=0;
00151 #endif
00152     }
00153 }