main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 
00004 #define Limit 100
00005 #define PI 3.14159265359
00006 
00007 int count_r;
00008 float rev;
00009 float rev_back = 0.0;
00010 float an_vel;
00011 float powe_element = 0.0;
00012 float erorr;
00013 float erorr_back = 0.0;
00014 float goal = 2.0 * PI;
00015 float k_p = 20.0;
00016 float k_i = 3.0;
00017 float k_d = 2.5;
00018 float integ;
00019 float deriva;
00020 int power;
00021 
00022 Ticker warikomi;
00023 
00024 int dir;
00025 //int vel = 130;
00026 
00027 Serial pc(USBTX, USBRX, 115200);
00028 RawSerial saber(p13,p14,115200);
00029 Timer t;
00030 QEI motor(p30, p29, NC, 100, &t, QEI::X4_ENCODING);
00031 
00032 
00033 void func()
00034 {
00035     count_r = motor.getPulses();
00036     rev = (count_r / 400.0) * 2.0* PI;
00037     an_vel = (rev - rev_back) / 0.01f;
00038     rev_back = rev;
00039 
00040     //////////////////////////////////
00041 
00042     erorr = goal - rev;
00043     integ += (erorr + erorr_back) * (1/2) * 0.01f;
00044     deriva = (erorr - erorr_back) / 0.01f;
00045     powe_element = k_p * erorr + k_i * integ + k_d * deriva;
00046     erorr_back = erorr;
00047 
00048     /////////////////////////////////////////////////////////
00049 
00050     if (31.0 >= powe_element && powe_element >= 0.0) {
00051         dir = 0;
00052     } else if (powe_element < 0.0) {
00053         dir = 1; 
00054     }
00055 
00056     power = powe_element / 31.0f * Limit;
00057     power = (int)power;
00058     
00059     if (power > 100){
00060         power = 100;
00061         }
00062 
00063     /////////////////////////////////////
00064 
00065     saber.putc(130);//アドレス
00066     saber.putc(dir);//回転方向
00067     saber.putc(abs(power));//出力0~127
00068     saber.putc((abs(power) + dir + 130) &0b1111111);
00069 }
00070 
00071 int main()
00072 {
00073     warikomi.attach(&func, 0.01);
00074     saber.baud(115200);
00075     while(1) {
00076         pc.printf("count: %d  count(float): %f   rev: %f  an_vel: %f  powe_element: %f  power: %d  dir: %d  erorr: %f\n"
00077                   , count_r, (float)count_r, rev, an_vel, powe_element, power, dir, erorr);
00078                   wait(0.01);
00079     }
00080 }