main.cpp

00001 #include "mbed.h"
00002 #include "RoboClaw.h"
00003 #include "QEI.h"
00004 #include "Filter.h"
00005 
00006 Filter filter(0.01);
00007 Timer t;
00008 QEI motor(p21, p22, NC, 500, &t, QEI::X4_ENCODING);
00009 RoboClaw robo(128, 115200, p9, NC);
00010 Serial pc(USBTX, USBRX, 115200);
00011 Ticker flipper;
00012 
00013 int count_r;
00014 int pre_count = 0;
00015 int error_PID;
00016 int goal = 3000;
00017 
00018 double k_p = 0.00035;
00019 double k_i = 0.00075;
00020 double k_d = 0.0003;
00021 
00022 double pro;
00023 double integ;
00024 double diff;
00025 double s_t = 0.01;
00026 
00027 double def;
00028 int s_d = 0.0;
00029 
00030 bool flag_check = false;
00031 bool flag_printf = false;
00032 
00033 int count_t = 0;
00034 
00035 void func()
00036 {
00037     //goal = (int)filter.SecondOrderLag(3000);
00038     count_t++;
00039     count_r = motor.getPulses();
00040     error_PID = (double)(goal - count_r);
00041     integ += (double)(count_r + pre_count) / 2.0 * s_t;
00042     diff = (count_r - pre_count) / s_t;
00043     def = k_p * error_PID + k_i * integ + k_d * diff;
00044 
00045     s_d = (int)def;
00046     if(s_d > 30) {
00047         s_d = 30;
00048     }
00049 
00050     if(s_d < -30) {
00051         s_d = -30;
00052     }
00053 
00054     flag_check = true;
00055 
00056     if(count_t >= 10) flag_printf = true;
00057 
00058     pre_count = count_r;
00059 }
00060 
00061 int main()
00062 {
00063     filter.setSecondOrderPara(1.0, 1.0, 0.0);
00064     flipper.attach(&func, 0.01);
00065 
00066     while(1) {
00067 
00068         if (flag_check == true) {
00069             if (error_PID <= 0) {
00070                 robo.ForwardM2(s_d);
00071                 flag_check = false;
00072             } else {
00073                 robo.BackwardM2(-1 * s_d);
00074                 flag_check = false;
00075             }
00076         }
00077         if(flag_printf == true) {
00078             pc.printf("count_r : %d\t\terror : %d\t\ts_d : %d\t\tgoal : %d\n", count_r, error_PID, s_d, goal);
00079             flag_printf = false;
00080             count_r = 0;
00081         }
00082 
00083     }
00084 }