Makoto Kobayashi
機械工学実験1作業用
Dependencies: PID QEI USBHost mbed
Revision 0:d7629adcea6d, committed 2014-10-20
- Comitter:
- neoqased
- Date:
- Mon Oct 20 11:15:26 2014 +0000
- Child:
- 1:36385b59d183
- Commit message:
- ??????1???
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Mon Oct 20 11:15:26 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Mon Oct 20 11:15:26 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/USBHost.lib Mon Oct 20 11:15:26 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/USBHost/#45dcbf2ba11d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Oct 20 11:15:26 2014 +0000
@@ -0,0 +1,463 @@
+//※重要※ 日本語コメントアウトは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
+
+/**************************入出力ポート設定**************************/
+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を記録するための構造体
+ float t;
+ float 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 * 2; //減速比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)] = {};
+ //初期化
+ 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);
+ exp[i+1].t = timer.read();
+ exp[i+1].rpm = (float)encoder.getPulses() / 2 / PULSE_PER_REVOLUTION * 60 / (exp[i+1].t - exp[i].t) * 2; //減速比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 , %f\n", exp[i].t, 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, ki = 0, kd = 0, max_rpm = 0, target_rpm = 0, reduction = 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;
+ ki = KI;
+ kd = KD;
+ max_rpm = MAX_RPM;
+ target_rpm = TARGET_RPM;
+
+ PID pid(kp, ki, kd, 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秒間実行する
+ exp[i+1].t = timer.read();
+ exp[i+1].rpm = (float)encoder.getPulses() / 2 / PULSE_PER_REVOLUTION * 60 / (exp[i+1].t - exp[i].t) * 2;
+ pid.setProcessValue(exp[i+1].rpm);
+ 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 = %f ki = %f kd = %f\n",kp, ki, kd);
+ fprintf(fp,"Time , RPM\n");
+ for(int i = 0; i <= 15*1000/PID_RATE; i++){
+ fprintf(fp,"%f , %f\n", exp[i].t, 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';
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Oct 20 11:15:26 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/5e5da4a5990b \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/setting.h Mon Oct 20 11:15:26 2014 +0000 @@ -0,0 +1,10 @@ +#ifndef _SETTING_H +#define _SETTING_H + +#define KP 0.00135 +#define KI 0.00962 +#define KD 0.0 +#define TARGET_RPM 4000 +#define MAX_RPM 10000 + +#endif \ No newline at end of file