main.cpp

00001 #include "mbed.h"
00002 #include "RoboClaw.h"
00003 #define PG 200//目標値
00004 #define PI 3.141592//円周率
00005 
00006 RoboClaw robo(130, 115200, p10, p9);//(address, rate, rx, tx)
00007 Serial pc(USBTX, USBRX, 115200);
00008 Ticker warikomi;//割り込み処理
00009 
00010 int count_r;//4x500
00011 
00012 float k_p_pos = 1;//比例ゲイン
00013 float k_i_pos = 0;//積分ゲイン
00014 float k_d_pos = 0;//微分ゲイン
00015 
00016 float error_pos;//位置PID制御の偏差
00017 float error_back_pos = 0;
00018 
00019 float rev;//回転数
00020 float rev_back = 0;
00021 
00022 float vel;//速度
00023 float vel_back = 0;
00024 
00025 float an_vel;//角速度
00026 float an_vel_back = 0;
00027 
00028 float dist;//移動距離
00029 float dist_back = 0;
00030 
00031 float integ;//積分
00032 float diff;//微分
00033 
00034 float element;//PIDの値
00035 float cmd;//roboclawに送る値
00036 
00037 bool flag = false;
00038 
00039 void func()
00040 {
00041     count_r = robo.ReadEncM1();//カウントの読み取り
00042     rev = count_r / 2000.0;//分解能はおそらく500
00043     dist = rev * 2.0 * PI * 67.0;//67はタイヤの半径(㎜)
00044     vel = (dist - dist_back) / 0.01;
00045     an_vel = (rev * 2.0 * PI - rev_back * 2.0 * PI) / 0.01;
00046 
00047     error_pos = PG - dist;
00048     integ += (error_back_pos + error_pos) / 2.0 * 0.01;
00049     diff = (error_pos - error_back_pos) / 0.01;
00050 
00051     element = k_p_pos * error_pos + k_i_pos * integ + k_d_pos * diff;
00052 
00053     cmd = element / 0.01;
00054 
00055     error_back_pos = error_pos;
00056     dist_back = dist;
00057     vel_back = vel;
00058     an_vel_back = an_vel;
00059 
00060     flag = true;
00061 }
00062 
00063 int main()
00064 {
00065     //robo.ResetEnc();
00066     warikomi.attach(&func, 0.01);
00067 
00068     while(1) {
00069         pc.printf("count:%d  dist:%f  vel:%f  an_vel:%f  rev:%f  error:%f\n", count_r, dist, vel, an_vel, rev, error);
00070         if (flag == true) {
00071             robo.SpeedM1((int)cmd);
00072             flag = false;
00073         }
00074         wait(0.01);
00075     }
00076 }