main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #define PI 3.14159265359
00004 
00005 Ticker warikomi;
00006 Serial pc(USBTX, USBRX);
00007 Timer t;
00008 QEI motor (D10,D5,NC,/*分解能*/,&t,QEI::X4_ENCODING);
00009 
00010 //DigitalIn A(D10);
00011 //DigitalIn B(D5);
00012 PwmOut motor_r(D9);
00013 DigitalOut direct(D2);
00014 float angle,vel, angle_back = 0,goal = 2 * PI,k_p = 0.8,erorr,duty = 0; //P制御
00015 float k_d = 0.05,deriva,erorr_back = 0;  //D制御
00016 float k_i = 0.0, integ; //I制御
00017 
00018 float erorr_vel ,erorr_vel_back = 0 ,deriva_vel ,integ_vel ,goal_vel = 10.0 ,duty_vel = 0;
00019 float k_p_vel = 0 ,k_d_vel = 0 ,k_i_vel = 0; 
00020 
00021 
00022 
00023 int count ;
00024 void func()
00025 {
00026 
00027 
00028     count = motor.getPulses();
00029     angle = (float)count * PI / 720.0;
00030     vel = (angle - angle_back) / 0.01;
00031     erorr = goal - angle;
00032     deriva = (erorr - erorr_back) / 0.01;
00033     integ += (erorr + erorr_back) * (1/2) * 0.01;
00034     angle_back = angle;
00035     
00036     erorr_vel = goal_vel - vel;
00037     deriva_vel = (erorr_vel - erorr_vel_back) / 0.01;
00038     integ_vel += (erorr_vel + erorr_vel_back) / (1/2) * 0.01;
00039     
00040 
00041     /*erorr_An = erorr;
00042     integ_An = integ ;
00043     deriva_An = deriva ;*/
00044     duty = erorr * k_p + k_d * deriva + k_i * integ;
00045     duty_vel =+ erorr_vel * k_p_vel + deriva_vel * k_d_vel + integ_vel * k_i_vel;
00046     erorr_back = erorr;
00047     erorr_vel_back = erorr_vel;
00048 }
00049 
00050 int main()
00051 {
00052     //pc.printf("check");
00053     wait_ms(2000);
00054     warikomi.attach(&func,0.01);
00055     // int count = 0;
00056     int back_A,back_B,Ach,Bch;
00057     while(1) {
00058 
00059         if (duty >= 0) {
00060             direct = 0;
00061             motor_r = duty;
00062             motor_r = duty_vel;
00063         } else if (duty < 0) {
00064             direct = 1;
00065             motor_r =-1 * duty ;
00066             motor_r =-1 * duty_vel;
00067             // duty = duty * 0.1;
00068 
00069         }
00070         /*while(1){
00071 
00072             if (angle > 2 * PI ) {
00073                 direct = 1;
00074                 motor_r = duty;
00075             }
00076         if (angle <= 2 *PI) {
00077             break;
00078         }
00079         }*/
00080 
00081 
00082 
00083 
00084         /* if (duty > 0.5) {
00085              duty = 0;
00086              }*/
00087 
00088         /* if (angle == 2 * PI) {
00089              duty = 0;
00090              }*/
00091 
00092         /*Ach = A.read();
00093         Bch = B.read();*/
00094         pc.printf("%d %f  現在の角度は%frad  角速度は%frad/sec  現在のデューティ比は%f  erorr:%f\r\n",
00095                   /*Ach ,Bch ,back_A ,back_B ,*/count,(float)count / 1440,angle,vel,duty,erorr);
00096         /*if (Bch == back_A ) {
00097             count++;
00098         }
00099         if (Ach == back_B) {
00100             count--;
00101         }
00102         
00103         
00104         back_A = Ach;
00105         back_B = Bch;*/
00106         
00107         wait_ms(1);
00108     }
00109 }