File content as of revision 0:14da14ed70e0:

#include "mbed.h"
#include "RoboClaw.h"
#define PG 200//目標値
#define PI 3.141592//円周率

RoboClaw robo(130, 115200, p10, p9);//(address, rate, rx, tx)
Serial pc(USBTX, USBRX, 115200);
Ticker warikomi;//割り込み処理

int count_r;//4x500

float k_p_pos = 1;//比例ゲイン
float k_i_pos = 0;//積分ゲイン
float k_d_pos = 0;//微分ゲイン

float error_pos;//位置PID制御の偏差
float error_back_pos = 0;

float rev;//回転数
float rev_back = 0;

float vel;//速度
float vel_back = 0;

float an_vel;//角速度
float an_vel_back = 0;

float dist;//移動距離
float dist_back = 0;

float integ;//積分
float diff;//微分

float element;//PIDの値
float cmd;//roboclawに送る値

bool flag = false;

void func()
{
    count_r = robo.ReadEncM1();//カウントの読み取り
    rev = count_r / 2000.0;//分解能はおそらく500
    dist = rev * 2.0 * PI * 67.0;//67はタイヤの半径(㎜)
    vel = (dist - dist_back) / 0.01;
    an_vel = (rev * 2.0 * PI - rev_back * 2.0 * PI) / 0.01;

    error_pos = PG - dist;
    integ += (error_back_pos + error_pos) / 2.0 * 0.01;
    diff = (error_pos - error_back_pos) / 0.01;

    element = k_p_pos * error_pos + k_i_pos * integ + k_d_pos * diff;

    cmd = element / 0.01;

    error_back_pos = error_pos;
    dist_back = dist;
    vel_back = vel;
    an_vel_back = an_vel;

    flag = true;
}

int main()
{
    //robo.ResetEnc();
    warikomi.attach(&func, 0.01);

    while(1) {
        pc.printf("count:%d  dist:%f  vel:%f  an_vel:%f  rev:%f  error:%f\n", count_r, dist, vel, an_vel, rev, error);
        if (flag == true) {
            robo.SpeedM1((int)cmd);
            flag = false;
        }
        wait(0.01);
    }
}