機械工学実験1作業用
Dependencies: PID QEI USBHost mbed
main.cpp
- Committer:
- neoqased
- Date:
- 2015-04-20
- Revision:
- 1:36385b59d183
- Parent:
- 0:d7629adcea6d
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'; }