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;
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     pc.baud(115200);
00041     return 0;
00042 }
00043 
00044 
00045 
00046 void run()
00047 {
00048     double motor;
00049     double b_r_c;
00050     double PID_v;
00051     double error , P, I, D, kp, r_kp;
00052     double last_error=0.0 ,last_I=0.0, last_brc, brc_df;
00053     bool first_time=true, r_kp_neg=false;
00054 
00055     while(1) {
00056 #ifdef task_ma_time
00057         task_pin=1;
00058 #endif
00059 
00060         cam.read();
00061 #ifdef Debug_cam_uart
00062 #ifdef L_eye
00063         for(int i=0; i<128; i++) {
00064             if(i==64)
00065                 pc.printf("X");
00066             else
00067                 pc.printf("%c", cam.sign_line_imageL[i]);
00068         }
00069         pc.printf("           ||             ");
00070 #endif
00071 #ifdef R_eye
00072         for(int i=128; i>=0; i--) {
00073             if(i==64)
00074                 pc.printf("X");
00075             else if(i<10)
00076                 pc.printf("-");
00077             else if(i>117)
00078                 pc.printf("-");
00079             else
00080                 pc.printf("%c", cam.sign_line_imageR[i]);
00081         }
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 #endif
00091 
00092         b_r_c=(double)cam.black_centerR();
00093         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);
00094 
00095         //compute
00096         //stand at 65
00097         error=b_r_c-offset;
00098         brc_df=b_r_c-last_brc;
00099         last_brc=b_r_c;
00100         
00101         if(first_time==true){
00102                 r_kp=error;
00103                 first_time=false;
00104             }
00105         else{
00106                 r_kp+=df;
00107             }
00108             
00109         if(r_kp<0.0){
00110                 r_kp*=-1.0;
00111                 r_kp_neg=true;
00112             }
00113         kp=0.025/(4000/r_kp);
00114         if(r_kp_neg==true){
00115                 r_kp_neg=false;
00116                 r_kp*=-1.0;
00117             } 
00118                 
00119         P=kp*error;
00120 
00121         I=last_I*2.0/3.0+error;
00122         last_I=I;
00123         I=(kp*0.02/0.04)*I;
00124 
00125         D=error-last_error;
00126         last_error=error;
00127         D=(kp*0.04/0.02)*D;
00128 
00129         PID_v=P+D;
00130 
00131 
00132         if(PID_v < 0.0) {
00133             PID_v*=-1;
00134             PID_v=0.085-(PID_v);
00135         } else if(PID_v > 0.0)
00136             PID_v=0.085+(PID_v);
00137         else
00138             PID_v=0.085;
00139 
00140         servo.set_angle(PID_v);
00141 
00142 #ifdef task_ma_time
00143         task_pin=0;
00144 #endif
00145     }
00146 }