File content as of revision 1:36385b59d183:

//※重要※  日本語コメントアウトはmbedのBeta-japaneseに登録すると使えるようになる(2014年現在)
//めんどくさいからファイル分割しないよ．熱意がある人はやって頂戴 by小林2014-9-12
#include "mbed.h"           //updateのマークが出ていてもupdateしないように．なんか仕様が変わってるっぽいからエラー吐くようになる
#include "QEI.h"
//#include "USBHostMSD.h"     //updateのマークが出ていてもupdateしないように．なんか仕様が変わってるっぽいからエラー吐くようになる
#include "PID.h"
#include "setting.h"

//1回転あたりのパルス
#define PULSE_PER_REVOLUTION 200
//PIDのウェイト ミリ秒(制御周期)
#define PID_RATE 10
//ギア比
#define GEAR_RATIO 2

#define KP_SCALE 10
#define TI_SCALE 20

/**************************入出力ポート設定**************************/
DigitalOut led1(LED1);      //mbed上LED出力4つ
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
DigitalIn dip1(p5);         //DIPスイッチの入力4つ
DigitalIn dip2(p6);
DigitalIn dip3(p7);
DigitalIn dip4(p8);
DigitalIn startsw(p9);      //スタートスイッチ入力
DigitalOut buzzer(p18);     //電子ブザー用デジタル出力
AnalogIn volume(p20);       //ポテンショメータ入力(アナログ)
PwmOut pwm(p25);            //モータへのPWM出力

Serial pc(USBTX, USBRX);    //PCとのシリアル通信設定
QEI encoder(p30, p29, NC, PULSE_PER_REVOLUTION, QEI::X2_ENCODING);        //QEIの設定.
Timer timer;                //タイマー

typedef struct{             //時間tとその時のrpmを記録するための構造体
    unsigned short t;
    short rpm;
} result;

/**************************プロトタイプ宣言**************************/
int Init();
int DipLed();
int GetDipValue();
int SW();
int Buzzer(int buzvar);
int find_header(FILE *fp, char keyword[]);
void rm_right_space(char str[]);

/**************************main文**************************/
int main() {
    //初期化
    Buzzer(2);
    //USBHostMSD msd("usb");
    LocalFileSystem local("local");
    Init();             //初期化関数
    int flag = 0;       //フラグ用変数
    //float vol = 0;      //ポテンショメータからの入力値を受け取る変数
    //float rpm = 0;      //rpmを格納するための変数
    //float t = 0;        //時間計測のための変数
    //result exp2[(int)(15*1000/PID_RATE+1)] = {};  //モード2用の構造体
    //result exp3[(int)(15*1000/PID_RATE+1)] = {};  //モード3用の構造体
    result exp[(int)(15*1000/PID_RATE+1)] = {};
    //メインループ
    while(1){
        Init();
        for(int i = 0; i <= 15*1000/PID_RATE; i++){
            exp[i].t = 0;
            exp[i].rpm = 0;
        }
        SW();           //スタートスイッチの入力を待つ
        flag = GetDipValue();   //DIPスイッチの入力を見る
        DipLed();               //DIPスイッチのHLでLEDを光らせる
        Buzzer(1);      //ブザーを鳴らす．
        switch (flag){      //実験モード切り替えのswitch文．ここにcase0~15を書き足せばモードが追加できる．
            case 0:     //DIPスイッチの入力なしで何もしない
                break;
            case 1:{        //モード1はポテンショメータからの入力で直接PWMのデューティ比を変更する．約1秒ごとにPCに値を出力する．このモードは時間で終了せず永遠に続く．終了するにはmbedのresetボタンを押す．
                //初期化
                float vol = 0;      //ポテンショメータからの入力値を受け取る変数
                float rpm = 0;      //rpmを格納するための変数
                float t = 0;        //時間計測のための変数
                int i1 = 0;
                encoder.reset();
                timer.reset();
                timer.start();
                //ループ(抜け出せない)
                while(1){
                    vol = 1 - volume;
                    pwm = vol;
                    if(i1 >= 20){
                        t = timer.read();
                        rpm = (float)encoder.getPulses() / 2 / PULSE_PER_REVOLUTION * 60 / t * GEAR_RATIO;  //減速比2
                        pc.printf("Duty Ratio = %.3f , %6d RPM\n", vol, (int)rpm);
                        encoder.reset();
                        i1 = 0;
                        timer.reset();
                    }
                    wait_ms(50);
                    i1++;
                }
                break;
            }
            case 2:{         //モード2はモータの応答を調べる．デューティ比1でモータを回転させ，100msごとの経過時間とRPMをUSBメモリのcsvファイルに出力する．約15秒で終了する．
                //result exp2[(int)(15*1000/PID_RATE+1)] = {};
                float dt = 0, tnow = 0, tpre = 0;
                //初期化
                exp[0].t = 0;
                exp[0].rpm = 0;
                encoder.reset();
                timer.reset();
                timer.start();
                pwm = 1;        //モータ回転開始!!
                //ループ(約15秒で終了)
                for(int i = 0; i < 15*1000/PID_RATE; i++){       //10msごとに時間とrpmの取得
                    wait_ms(PID_RATE);
                    tpre = tnow;
                    tnow = timer.read();
                    exp[i+1].t = (unsigned short)(tnow * 1000);
                    dt = tnow - tpre;
                    exp[i+1].rpm = (short)((float)encoder.getPulses() / 2 / PULSE_PER_REVOLUTION * 60 / dt * GEAR_RATIO);        //減速比2
                    encoder.reset();
                }
                for(int i = 100; i >= 0; i--){      //ゆるやかに減速
                    pwm = (float)i / 100;
                    wait_ms(20);
                }
                pwm = 0;                            //回転を止める
                wait(1);
                FILE * fp = fopen("/local/exp2.csv","w");
                if(fp == NULL){     //ファイルオープンエラー
                    Buzzer(-1);     //エラーコード-1のブザーを鳴らす
                    break;          //終了
                }
                fprintf(fp,"Time , RPM\n");
                for(int i = 0; i <= 15*1000/PID_RATE; i++){
                    fprintf(fp,"%f , %d\n", (float)exp[i].t / 1000.0, exp[i].rpm);        //ファイルに実験データの書き込み
                }
                fclose(fp);
                Buzzer(4);          //終了を知らせる
                break;
            }
            case 3:{         //モード3はモータの応答を調べる．PID制御をかけ，経過時間とRPMをUSBメモリのcsvファイルに出力する．約15秒で終了する．
                //result exp3[(int)(15*1000/PID_RATE+1)] = {};
                float kp = 0, ti = 0, td = 0, max_rpm = 0, target_rpm = 0, reduction = 0, dt = 0, tnow = 0, tpre = 0, rpmnow = 0;
                //int pulse[15*1000/PID_RATE+2] = {};
                //int j = 0;
                //float times = 0;
                encoder.reset();
                timer.reset();
                /*
                -----PID制御の係数設定ファイルpid.txtの書き方-----
                #kp
                1.0
                
                #ki
                1.0
                
                #kd
                0.0
                
                #max_rpm
                10000
                エンコーダで読めるモータのrpmの最大値．これを超えないように制御を行う．
                
                #target_rpm
                3000
                http://denki.nara-edu.ac.jp/~yabu/soft/header.htmlを参考にしました
                */
                
                /*FILE *fppid = fopen("/local/pid.txt","r");
                if(fppid == NULL){
                    Buzzer(-2);
                    break;
                }*/
                //fscanf(fppid,"%f %f %f %f %f", &kp, &ki, &kd, &max_rpm, &target_rpm);
                //fclose(fppid);
                //find_header(fppid,"# kp");       //PID制御用の係数の読み込み
                //fscanf(fppid,"%f",&kp);
                //find_header(fppid,"# ki");
                //fscanf(fppid,"%f",&ki);
                //find_header(fppid,"# kd");
                //fscanf(fppid,"%f",&kd);
                //find_header(fppid,"# max_rpm");
                //fscanf(fppid,"%f",&max_rpm);
                //find_header(fppid,"# target_rpm");
                //fscanf(fppid,"%f",&target_rpm);
                //fclose(fppid);
                
                kp = KP / KP_SCALE;
                ti = TI * TI_SCALE;
                td = TD;
                max_rpm = MAX_RPM;
                target_rpm = TARGET_RPM;
                
                PID pid(kp, ti, td, PID_RATE);      //PIDの設定
                pid.setInputLimits(0.0, max_rpm);
                pid.setOutputLimits(0.0, 1.0);
                pid.setMode(AUTO_MODE);
                pid.setSetPoint(target_rpm);
                
                exp[0].t = 0;
                exp[0].rpm = 0;
                timer.start();
                for(int i = 0; i < 15*1000/PID_RATE; i++){        //15秒間実行する
                    tpre = tnow;
                    tnow = timer.read();
                    exp[i+1].t = (unsigned short)(tnow * 1000);
                    dt = tnow - tpre;
                    rpmnow = (float)encoder.getPulses() / 2 / PULSE_PER_REVOLUTION * 60 / dt * GEAR_RATIO;
                    exp[i+1].rpm = (short)rpmnow;
                    pid.setProcessValue(rpmnow);
                    pwm = pid.compute();
                    encoder.reset();
                    //pulse[i+1] = encoder.getPulses() / 2.0;
                    //pid.setProcessValue((float)(pulse[i+1] - pulse[i]) / PULSE_PER_REVOLUTION * 60 / (timer.read() - times) * 2);
                    //times = timer.read();
                    //if((i % (int)(100 / PID_RATE) == 0) && (i != 0)){   //100ms毎に時間とrpmを読み込む
                        //j++;
                        //exp3[j].t = timer.read();
                        //exp3[j].rpm = (float)(pulse[i+1] - pulse[i+1-(int)(100/PID_RATE)]) / PULSE_PER_REVOLUTION * 60 / (exp3[j].t - exp3[j-1].t) * 2;
                    //}
                    wait_ms(PID_RATE);
                }
                reduction = pwm.read();
                for(int i = 0; i <= 100; i++){
                    pwm = pwm.read() - reduction / 100;
                    wait_ms(20);
                }
                pwm = 0;
                wait(1);
                FILE *fp = fopen("/local/exp3.csv","w");
                if(fp == NULL){     //ファイルオープンエラー
                    Buzzer(-1);     //エラーコード-1のブザーを鳴らす
                    break;          //終了
                }
                fprintf(fp,"kp = %.6f  ki = %.6f  kd = %.6f\n", (float)KP, (float)TI, (float)TD);
                fprintf(fp,"Time , RPM\n");
                for(int i = 0; i <= 15*1000/PID_RATE; i++){
                    fprintf(fp,"%f , %d\n", (float)exp[i].t / 1000.0, exp[i].rpm);        //ファイルに実験データの書き込み
                }
                fclose(fp);
                Buzzer(5);          //終了を知らせる
                break;
            }
            default:        //0と上記以外でなにもなし
                break;
        }
    }
}

/**************************関数たち**************************/
int Init(){         //初期化関数
    pwm = 0;
    led1 = 0;
    led2 = 0;
    led3 = 0;
    led4 = 0;
    buzzer = 0;
    pc.baud(9600);
    encoder.reset();
    timer.reset();
    pwm.period_us(25);      //40kHz
    DipLed();
    return 0;
}

int DipLed(){       //DIPスイッチの状態によってledを光らせる関数
    if(dip1 == 1) led1 = 1; else led1 = 0;     //DIPスイッチの1がHならLEDを光らせる,Lなら光らせない
    if(dip2 == 1) led2 = 1; else led2 = 0;
    if(dip3 == 1) led3 = 1; else led3 = 0;
    if(dip4 == 1) led4 = 1; else led4 = 0;
    return 0;
}

int GetDipValue(){  //DIPスイッチの値を取得し，値を返す関数
    int Dip1 = dip1, Dip2 = dip2, Dip3 = dip3, Dip4 = dip4; //DIPスイッチの値を取得
    if(Dip1 == 0){
        if(Dip2 == 0){
            if(Dip3 == 0){
                if(Dip4 == 0) return 0; else return 1;      //DIPスイッチが0000の場合0を返す．0001なら1
            }else{
                if(Dip4 == 0) return 2; else return 3;
            }
        }else{
            if(Dip3 == 0){
                if(Dip4 == 0) return 4; else return 5;
            }else{
                if(Dip4 == 0) return 6; else return 7;
            }
        }
    }else{
        if(Dip2 == 0){
            if(Dip3 == 0){
                if(Dip4 == 0) return 8; else return 9;
            }else{
                if(Dip4 == 0) return 10; else return 11;
            }
        }else{
            if(Dip3 == 0){
                if(Dip4 == 0) return 12; else return 13;
            }else{
                if(Dip4 == 0) return 14; else return 15;
            }
        }
    }
}

int SW(){        //スタートスイッチ用関数，押して離したらスタート
    int i = 0, j = 0;
    while(i < 3){       //チャタリング除去，15msにわたってスタートスイッチが押されていればbreak
        if(startsw == 1) i++;
        else i = 0;
        DipLed();
        wait_ms(5);
    }
    while(j < 3){       //上に同じ，スタートスイッチが離されたことを検知
        if(startsw == 0) j++;
        else j = 0;
        DipLed();
        wait_ms(5);
    }
    return 0;
}

int Buzzer(int buzvar){     //電子ブザーを鳴らす関数
    switch (buzvar){
        /**************エラーを知らせるbeep**************/
        case -3:        //error * - -
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            break;
        case -2:        //error * - - -
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            break;
        case -1:        //error * - - - -
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            break;
        /**************エラーここまで**************/
        case 0:         //サウンドなし
            buzzer = 0;
            break;
        /**************状態を知らせるためのbeep**************/
        case 1:         // *(短)
            buzzer = 1; wait(0.1); buzzer = 0;
            break;
        case 2:         // * *
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.05);
            buzzer = 1; wait(0.1); buzzer = 0;
            break;
        case 3:         // -(長)
            buzzer = 1; wait(0.3); buzzer = 0;
            break;
        case 4:         // - -
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.3);
            buzzer = 1; wait(0.3); buzzer = 0;
            break;
        case 5:         // ---
            buzzer = 1; wait(0.9); buzzer = 0;
            break;
        case 6:         // * * *  * * *  * * *  *
            for(int i = 0; i < 3; i++){
                for(int j = 0; j < 3; j++){
                    buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
                }
                wait(0.2);
            }
            buzzer = 1; wait(0.1); buzzer = 0;
            break;
        case 7:         // **-* ** -* ** *** **** "finish"
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            wait(0.2);
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            wait(0.2);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            wait(0.2);
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            wait(0.2);
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            wait(0.2);
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0;
            break;
        case 8:         // *-*** -*- --* "オワリ"
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.1);
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            wait(0.2);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
            wait(0.2);
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 
            buzzer = 1; wait(0.1); buzzer = 0;
            break;
        /**************状態を知らせるためのbeepここまで**************/
        default:        //no sound
            buzzer = 0;
            break;
    }
    return 0;
}


int find_header(FILE *fp, char keyword[])
/*
    入力     fp        ファイルポインタ
             keyword   キーワード
    戻り値             1 : ヘッダを見つけた   0 ヘッダが見つからなかった
*/
{
    char char_buf[256];
    char keyword_buf[256];
    char *condition;
    int  len,i;

    strcpy(keyword_buf,keyword);
    rm_right_space(keyword_buf);  /* 右側の空白を取り除く */

    rewind(fp);
    for(;;){
        condition = fgets(char_buf,BUFSIZ,fp);
        if ( condition == NULL ) break;
        rm_right_space(char_buf);       /* 右側の空白と改行記号を取り除く */
        len = strlen(char_buf);
        if ( len == 0 ) continue;       /* この文はなくてもよい */
        for ( i = 0 ; i < len ; i++ ){  /* 左側の空白を取り除く */
            if ( char_buf[i] != ' ' ) break;
        }
        if ( strcmp(&char_buf[i],keyword_buf)==0){
            return(1);
            break;
        }
    }
    fprintf(stderr,"cannot find header.  key word : |%s|\n",keyword);
    Buzzer(-3);
    return(0);
}

/* 文字列の右側の空白を削除する    改行記号がある場合はそれも除去する */

void rm_right_space(char str[])
{
    int i;
    
    i = strlen(str)-1;
    if ( str[i] == '\n' ) i--;

    while(i>=0){
        if ( str[i] != ' ')  break;
        i--;
    }
    str[i+1] = '\0';
}