File content as of revision 1:62b321f6c9c3:

/* タイマ割り込みを使用して回転数(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);
DigitalOut myled(D13);

/* 以下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;
Timer timer;

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 ice(int address, int data)
{
    //周波数: 100kHz
    //i2c.frequency(100000);
    
    // スタートコンディション出力 
    i2c.start();
    wait_us(20);
    
    // アドレスの書き込み    
    i2c.write(address);
    wait_us(20);
    
    // データの書き込み 
    i2c.write(data);
    wait_us(20);
    
    // ストップコンディション出力 
    i2c.stop();
    wait_us(90);
}
*/

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", 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_RF);
    //pc.printf("%lf\n", a_v);
    //pc.printf("rpm: %d  a_v: %lf\n", rpm[1], a_v);
}

void front_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: -
        //RB: -
        //LF: +
        //LB: +
        
        // 目標値 
        RF.setSetPoint(300);
        RB.setSetPoint(300);
        LF.setSetPoint(300);
        LB.setSetPoint(300);
        
        // センサ出力 
        RF.setProcessValue(rpm_RF);
        RB.setProcessValue(rpm_RB);
        LF.setProcessValue(rpm_LF);
        LB.setProcessValue(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];
}

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

        // よくわからんやつ 
        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(rpm_RF);
        RB.setProcessValue(rpm_RB);
        LF.setProcessValue(rpm_LF);
        LB.setProcessValue(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)
    {  
        timer.start();
        myled = 1;
        
        while(timer.read() <= 5.0f) {
            //timer.start();
            
            front_PID();
            /*
            ice(0xA0, 0x01);
            ice(0xA2, 0x02);
            ice(0xA4, 0x03);
            ice(0xA6, 0x04);
            */
            i2c.write(0xA0, true_RF_data, 1, false);     
            wait_us(20);       
            
            i2c.write(0xA2, true_RB_data, 1, false);
            wait_us(20);
            
            i2c.write(0xA4, true_LB_data, 1, false);
            wait_us(20);
            
            i2c.write(0xA6, true_LF_data, 1, false);
            wait_us(20);
            //wait_ms(50);
        }
        
        timer.reset();
        myled = 0;
        wait(1);
        
        RF.reset();
        RB.reset();
        LF.reset();
        LB.reset();
        wait(1);
        
        //wait(5);
        
        /*
        timer.start();
        while(timer.read() <= 5.0f) {
            //timer.start();
            back_PID();
            i2c.write(0xA0, RF_data, 1);
            i2c.write(0xA2, RB_data, 1);
            i2c.write(0xA4, LB_data, 1);
            i2c.write(0xA6, LF_data, 1);
            wait_ms(40);
        }
        timer.reset();
        */
        /*
        RF.reset();
        RB.reset();
        LF.reset();
        LB.reset();
        //wait(5);
        */
        
        
        /*
        // センサ出力値の最小、最大 
        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);
        }
        */
    }
}