File content as of revision 0:f73c1b076ae4:

/* タイマ割り込みを使用して回転数(RPS)を取得し表示するプログラム */
/* これまでの計算手順では回転数が取得できないことが判明し、改善済である */
/* 具体的には取得したパルス数を分解能で割り算→掛け算でRPSへ変換から */
/* 取得したパルス数を掛け算→分解能で割ってRPSへ変換としている。 */
/* ロリコンにはTIM2~TIM5がいいらしい(Nucleo_F401re) */
/* TIM2(CH1: NC, CH2: D3(PB_3), CH3: D6(PB_10), CH4: NC) */
/* TIM3(CH1: D5(PB_4), CH2: D9(PC_7), CH3: NC, CH4: NC) */
/* TIM4(CH1: D10(PB_6), CH2: NC, CH3: NC, CH4: NC) */
/* TIM5(CH1: NC, CH2: NC, CH3: NC, CH4: NC) */
/* 2018/6/9追記 */
/* ついに回転数の！PI制御を実現できました。 */
/* タイマ割込みで角速度を取得してみようと思います。 */

#include "mbed.h"
#include "math.h"
#include "QEI.h"
#include "PID.h"
#define PI  3.14159265359

PID controller(0.7, 0.7, 0.0, 0.001);
PID RF(0.5, 0.3, 0.0, 0.001);
PID RB(0.5, 0.3, 0.0, 0.001);
PID LF(0.5, 0.3, 0.0, 0.001);
PID LB(0.5, 0.3, 0.0, 0.001);
I2C i2c(PB_3, PB_10);  //SDA, SCL
//Serial pc(USBTX, USBRX);
DigitalOut emergency(PA_13);

/* 以下446基板用 */
QEI wheel_LB(PA_1, PA_0, NC, 624);
QEI wheel_RB(PB_6, PB_7, NC, 624);
QEI wheel_LF(PB_4, PB_5, NC, 624);
QEI wheel_RF(PC_8, PC_9, NC, 624);

Ticker get_RPS;
Ticker print_RPS;

int rps_RF;
int rps_RB;
int rps_LF;
int rps_LB;

int rpm_RF;
int rpm_RB;
int rpm_LF;
int rpm_LB;

int pulse_RF;
int pulse_RB;
int pulse_LF;
int pulse_LB;

int counter_RF;
int counter_RB;
int counter_LF;
int counter_LB;

double a_v; /* 角速度 */
double n_v; /* 速度(秒速) */ 
double h_v; /* 速度(時速) */ 
double n_a; /* 加速度 */

char send_data[1];
char true_send_data[1];

char RF_data[1];
char RB_data[1];
char LF_data[1];
char LB_data[1];
char true_RF_data[1];
char true_RB_data[1];
char true_LF_data[1];
char true_LB_data[1];

/* ロリコン処理関数 */
void flip();
/* RPS表示関数 */
void flip2();

void flip(){
    
    pulse_RF = wheel_RF.getPulses();
    pulse_RB = wheel_RB.getPulses();
    pulse_LF = wheel_LF.getPulses();
    pulse_LB = wheel_LB.getPulses();
        
    /* *rps変換 */
    /*10ms*100 = 1s
    counter_RB = pulse_RB * 100;
    counter_LB = pulse_LB * 100;
    */
    
    //40ms*25 = 1s
    counter_RF = pulse_RF * 25;
    counter_RB = pulse_RB * 25;
    counter_LF = pulse_LF * 25;
    counter_LB = pulse_LB * 25;
    
    /*
    //100ms*10 = 1s
    counter_RB = pulse_RB * 10;
    counter_LB = pulse_LB * 10;
    */

    /* /分解能 */
    rps_RF = counter_RF / 100;
    rps_RB = counter_RB / 100;
    rps_LF = counter_LF / 100;
    rps_LB = counter_LB / 100;
    
    rpm_RF = pulse_RF * 25 * 60 / 100;
    rpm_RB = pulse_RB * 25 * 60 / 100;
    rpm_LF = pulse_LF * 25 * 60 / 100;
    rpm_LB = pulse_LB * 25 * 60 / 100;
    
    /*
    rps[0] = counter_RB / 100;
    rps[1] = counter_LB / 100;
    
    rpm[0] = pulse_RB * 25 * 60 / 100;
    rpm[1] = pulse_LB * 25 * 60 / 100;
    */
    
    /* RPMから角速度へ変換 */
    a_v = rpm_LB * PI / 30;
    
    /* RPMから速度(秒速)へ変換 */
    /* タイヤ半径を0.05[m]とする */
    n_v = 0.05 * 2 * PI * rpm_LB / 60;
    
    /* 速度(秒速)から速度(時速)へ変換 */
    h_v = n_v * 3600 / 1000;
    
    //n_a = n_v / 
    
    /*
    if(rpm[1] < 200){
        send_data[0]--;
    }
    else if(rpm[1] > 250){
        send_data[1]++;
    }
    */
    
    //pc.printf("RF: %d   RB: %d  LF: %d  LB: %d\n", rpm_RF, rpm_RB, rpm_LF, rpm_LB);
    //pc.printf("%d\n", abs(rpm_RF));
    //pc.printf("%lf\n", n_v);
    //pc.printf("%lf\n", h_v);
    //pc.printf("rpm: %d  n_v: %lf\n", rpm[1], n_v);

    wheel_RF.reset();
    wheel_RB.reset();
    wheel_LF.reset();
    wheel_LB.reset();
}

void flip2()
{
    //pc.printf("RPS_RB: %d   RPS_LB: %d\n", abs(rps[0]), abs(rps[1]));
    //pc.printf("RPM_RB: %d   RPM_LB: %d\n", abs(rpm[0]), abs(rpm[1]));
    //pc.printf("%d\n", rpm[1]);
    //pc.printf("%lf\n", a_v);
    //pc.printf("rpm: %d  a_v: %lf\n", rpm[1], a_v);
}

void PID()
{ 
        // センサ出力値の最小、最大 
        //RF.setInputLimits(0, 1100);
        RF.setInputLimits(-400, 400);
        RB.setInputLimits(-400, 400);
        LF.setInputLimits(-400, 400);
        LB.setInputLimits(-400, 400);
        
        // 制御量の最小、最大 
        RF.setOutputLimits(0x01, 0x37);
        RB.setOutputLimits(0x01, 0x37);
        LF.setOutputLimits(0x01, 0x37);
        LB.setOutputLimits(0x01, 0x37);

        // よくわからんやつ 
        RF.setMode(AUTO_MODE);
        RB.setMode(AUTO_MODE);
        LF.setMode(AUTO_MODE);
        LB.setMode(AUTO_MODE);
        
        // 目標値 
        RF.setSetPoint(300);
        RB.setSetPoint(300);
        LF.setSetPoint(300);
        LB.setSetPoint(300);
        
        // センサ出力 
        RF.setProcessValue(abs(rpm_RF));
        RB.setProcessValue(abs(rpm_RB));
        LF.setProcessValue(abs(rpm_LF));
        LB.setProcessValue(abs(rpm_LB));
        
        // 制御量(計算結果) 
        RF_data[0] = RF.compute();
        RB_data[0] = RB.compute();
        LF_data[0] = LF.compute();
        LB_data[0] = LB.compute();
       
        // 制御量をPWM値に変換 
        true_RF_data[0] = 0x38 - RF_data[0];
        true_RB_data[0] = 0x38 - RB_data[0];
        true_LF_data[0] = 0x38 - LF_data[0];
        true_LB_data[0] = 0x38 - LB_data[0];
}

int main(void)
{   
    emergency = 0;
    send_data[0] = 0x40;
    i2c.write(0xA0, send_data, 1);
    i2c.write(0xA2, send_data, 1);
    i2c.write(0xA4, send_data, 1);
    i2c.write(0xA6, send_data, 1);
    wait(0.1);
    
    /* 加減速処理を入れた場合処理サイクルの最小周期は40msであった(2018/5/12) */
    get_RPS.attach_us(&flip, 40000);
    //get_RPS.attach_us(&flip, 100000);

    //RPS表示(0.1sサイクル)
    //print_RPS.attach(&flip2, 0.1);

    while(1)
    {  
        
        PID();
        i2c.write(0xA0, true_RF_data, 1);
        i2c.write(0xA2, true_RB_data, 1);
        i2c.write(0xA4, true_LB_data, 1);
        i2c.write(0xA6, true_LF_data, 1);
        
        /*
        // センサ出力値の最小、最大 
        RF.setInputLimits(0, 1100);
        RB.setInputLimits(0, 1100);
        LF.setInputLimits(0, 1100);
        LB.setInputLimits(0, 1100);
        
        // 制御量の最小、最大 
        RF.setOutputLimits(0x01, 0x37);
        RB.setOutputLimits(0x01, 0x37);
        LF.setOutputLimits(0x01, 0x37);
        LB.setOutputLimits(0x01, 0x37);

        // よくわからんやつ 
        RF.setMode(AUTO_MODE);
        RB.setMode(AUTO_MODE);
        LF.setMode(AUTO_MODE);
        LB.setMode(AUTO_MODE);
        
        // 目標値 
        RF.setSetPoint(400);
        RB.setSetPoint(400);
        LF.setSetPoint(400);
        LB.setSetPoint(400);
        
        // センサ出力 
        RF.setProcessValue(abs(rpm_RF));
        RB.setProcessValue(abs(rpm_RB));
        LF.setProcessValue(abs(rpm_LF));
        LB.setProcessValue(abs(rpm_LB));
        
        // 制御量(計算結果) 
        RF_data[0] = RF.compute();
        RB_data[0] = RB.compute();
        LF_data[0] = LF.compute();
        LB_data[0] = LB.compute();
       
        // 制御量をPWM値に変換 
        true_RF_data[0] = 0x38 - RF_data[0];
        true_RB_data[0] = 0x38 - RB_data[0];
        true_LF_data[0] = 0x38 - LF_data[0];
        true_LB_data[0] = 0x38 - LB_data[0];
        */
        
        /*
        // センサ出力値の最小、最大 
        controller.setInputLimits(0, 1100);
        
        // 制御量の最小、最大 
        controller.setOutputLimits(0x01, 0x37);
        
        // よくわからんやつ 
        controller.setMode(AUTO_MODE);
        
        // 目標値 
        controller.setSetPoint(400);
        
        // センサ出力 
        controller.setProcessValue(abs(rpm[1]));
        
        // 制御量(計算結果) 
        send_data[0] = controller.compute();
        
        // 制御量をPWM値に変換 
        true_send_data[0] = 0x38 - send_data[0];
        
        i2c.write(0x88, true_send_data, 1);
        */
        
        /*
        //どんどん加速
        for(send_data[0] = 0x37; send_data[0] > 0x01; send_data[0]--){
            //ice(0x88, send_data);
            //ice(0xA2, send_data);
            i2c.write(0xA0, send_data, 1);
            i2c.write(0xA2, send_data, 1);
            i2c.write(0xA4, send_data, 1);
            i2c.write(0xA6, send_data, 1);
            wait(0.1);
        }
        
        //だんだん減速
        for(send_data[0] = 0x02; send_data[0] <= 0x37; send_data[0]++){
            //ice(0x88, send_data);
            //ice(0xA2, send_data);
            i2c.write(0xA0, send_data, 1);
            i2c.write(0xA2, send_data, 1);
            i2c.write(0xA4, send_data, 1);
            i2c.write(0xA6, send_data, 1);
            wait(0.1);
        }
        */
    }
}