File content as of revision 3:1223cab367d9:

/* タイマ割り込みを使用して回転数(RPS)を取得し表示するプログラム */
/* 吉田啓人氏が頑張って改善したMDプログラム用に改善しました */
/* 具体的には取得したパルス数を分解能で割り算→掛け算で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.4, 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);
Serial photo(D8, D2);
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 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;
    
    
    /* 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;
    
    //pc.printf("RF: %d   RB: %d  LF: %d  LB: %d\n", rpm_RF, rpm_RB, rpm_LF, rpm_LB);
    pc.printf("前: %d, 後: %d\n", rpm_LB, rpm_LF);
    //pc.printf("%d\n", rpm_LB);

    wheel_RF.reset();
    wheel_RB.reset();
    wheel_LF.reset();
    wheel_LB.reset();
}
int front_PID(int setpoint)
{ 
        // センサ出力値の最小、最大 
        RF.setInputLimits(-2000, 2000);
        RB.setInputLimits(-2000, 2000);
        LF.setInputLimits(-2000, 2000);
        LB.setInputLimits(-2000, 2000);
        
        // 制御量の最小、最大 
        RF.setOutputLimits(0x0C, 0x7C);
        RB.setOutputLimits(0x0C, 0x7C);
        LF.setOutputLimits(0x0C, 0x7C);
        LB.setOutputLimits(0x0C, 0x7C);

        // よくわからんやつ 
        RF.setMode(AUTO_MODE);
        RB.setMode(AUTO_MODE);
        LF.setMode(AUTO_MODE);
        LB.setMode(AUTO_MODE);
        
        // 目標値 
        RF.setSetPoint(setpoint);
        RB.setSetPoint(setpoint);
        LF.setSetPoint(setpoint);
        LB.setSetPoint(setpoint);
        
        // センサ出力 
        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] = 0x7D - RF_data[0];
        true_RB_data[0] = 0x7D - RB_data[0];
        true_LF_data[0] = 0x7D - LF_data[0];
        true_LB_data[0] = 0x7D - LB_data[0];
        
        return 0;
}
int back_PID(int setpoint1, int setpoint2)
{ 
        // センサ出力値の最小、最大 
        RF.setInputLimits(0, 2000);
        RB.setInputLimits(0, 2000);
        LF.setInputLimits(0, 2000);
        LB.setInputLimits(0, 2000);
        
        // 制御量の最小、最大 
        RF.setOutputLimits(0x84, 0xFF);
        RB.setOutputLimits(0x84, 0xFF);
        LF.setOutputLimits(0x84, 0xFF);
        LB.setOutputLimits(0x84, 0xFF);

        // よくわからんやつ 
        RF.setMode(AUTO_MODE);
        RB.setMode(AUTO_MODE);
        LF.setMode(AUTO_MODE);
        LB.setMode(AUTO_MODE);
        
        // 目標値 
        RF.setSetPoint(setpoint1);
        RB.setSetPoint(setpoint1);
        LF.setSetPoint(setpoint1);
        LB.setSetPoint(setpoint2);
        
        // センサ出力 
        //目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
        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();
       
        return 0;
}
int main(void)
{   
    emergency = 0;
    send_data[0] = 0x80;
    //i2c.write(0x10, send_data, 1);
    //i2c.write(0x12, send_data, 1);
    //i2c.write(0x14, send_data, 1);
    //i2c.write(0x16, send_data, 1);
    i2c.write(0x20, send_data, 1);
    i2c.write(0x22, send_data, 1);
    wait(0.1);
    
    /* 加減速処理を入れた場合処理サイクルの最小周期は40msであった(2018/5/12) */
    get_RPS.attach_us(&flip, 40000);
    //get_RPS.attach_us(&flip, 100000);
    
    while(1)
    {  
        /*
        front_PID(300);
        i2c.write(0x20, true_LB_data, 1, false);
        i2c.write(0x22, true_LF_data, 1, false);
        wait_us(20);
        */
        
        
        //back_PID(430, 730);
        //back_PID(300, 500);
        back_PID(325, 650);
        //back_PID(250, 900);
        //LB_data[0] = 0x0C;
        //LF_data[0] = 0x0C;
        i2c.write(0x10, LB_data, 1, false);
        i2c.write(0x22, LF_data, 1, false);
        wait_us(20);
        
        /*
        timer.start();        
        while(timer.read() <= 5.0f) {
            myled = 1;
            front_PID(200);
            i2c.write(0x20, true_LF_data, 1, false);
            wait_us(20);
        }
        LF.reset();
        while((timer.read() > 5.0f) && (timer.read() <= 10.0f)) {
            myled = 0;
            true_LF_data[0] = 0x80;
            i2c.write(0x20, true_LF_data, 1, false);
            wait_us(20);
        }
        while((timer.read() > 10.0f) && (timer.read() <= 15.0f)) {
            myled = 1;
            back_PID(200);
            i2c.write(0x20, LF_data, 1, false);
            wait_us(20);
        }
        LF.reset();
        while((timer.read() > 15.0f) && (timer.read() <= 20.0f)) {
            myled = 0;
            true_LF_data[0] = 0x80;
            i2c.write(0x20, true_LF_data, 1, false);
            wait_us(20);
        }
        timer.reset();
        */
        
        /*
        //どんどん加速(逆転)
        for(send_data[0] = 0x81; send_data[0] < 0xF5; 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);
            
            i2c.write(0x20, send_data, 1);
            wait(0.1);
        }
        //だんだん減速(逆転)
        for(send_data[0] = 0xF4; send_data[0] > 0x80; 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);
            
            i2c.write(0x20, send_data, 1);
            wait(0.1);
        }
        send_data[0] = 0x80;
        i2c.write(0x20, send_data, 1);
        wait(5);
        */
    }
}