Makoto Kobayashi
機械工学実験1作業用
Dependencies: PID QEI USBHost mbed
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 //※重要※ 日本語コメントアウトはmbedのBeta-japaneseに登録すると使えるようになる(2014年現在) 00002 //めんどくさいからファイル分割しないよ．熱意がある人はやって頂戴 by小林2014-9-12 00003 #include "mbed.h" //updateのマークが出ていてもupdateしないように．なんか仕様が変わってるっぽいからエラー吐くようになる 00004 #include "QEI.h" 00005 //#include "USBHostMSD.h" //updateのマークが出ていてもupdateしないように．なんか仕様が変わってるっぽいからエラー吐くようになる 00006 #include "PID.h" 00007 #include "setting.h" 00008 00009 //1回転あたりのパルス 00010 #define PULSE_PER_REVOLUTION 200 00011 //PIDのウェイト ミリ秒(制御周期) 00012 #define PID_RATE 10 00013 //ギア比 00014 #define GEAR_RATIO 2 00015 00016 #define KP_SCALE 10 00017 #define TI_SCALE 20 00018 00019 /**************************入出力ポート設定**************************/ 00020 DigitalOut led1(LED1); //mbed上LED出力4つ 00021 DigitalOut led2(LED2); 00022 DigitalOut led3(LED3); 00023 DigitalOut led4(LED4); 00024 DigitalIn dip1(p5); //DIPスイッチの入力4つ 00025 DigitalIn dip2(p6); 00026 DigitalIn dip3(p7); 00027 DigitalIn dip4(p8); 00028 DigitalIn startsw(p9); //スタートスイッチ入力 00029 DigitalOut buzzer(p18); //電子ブザー用デジタル出力 00030 AnalogIn volume(p20); //ポテンショメータ入力(アナログ) 00031 PwmOut pwm(p25); //モータへのPWM出力 00032 00033 Serial pc(USBTX, USBRX); //PCとのシリアル通信設定 00034 QEI encoder(p30, p29, NC, PULSE_PER_REVOLUTION, QEI::X2_ENCODING); //QEIの設定. 00035 Timer timer; //タイマー 00036 00037 typedef struct{ //時間tとその時のrpmを記録するための構造体 00038 unsigned short t; 00039 short rpm; 00040 } result; 00041 00042 /**************************プロトタイプ宣言**************************/ 00043 int Init(); 00044 int DipLed(); 00045 int GetDipValue(); 00046 int SW(); 00047 int Buzzer(int buzvar); 00048 int find_header(FILE *fp, char keyword[]); 00049 void rm_right_space(char str[]); 00050 00051 /**************************main文**************************/ 00052 int main() { 00053 //初期化 00054 Buzzer(2); 00055 //USBHostMSD msd("usb"); 00056 LocalFileSystem local("local"); 00057 Init(); //初期化関数 00058 int flag = 0; //フラグ用変数 00059 //float vol = 0; //ポテンショメータからの入力値を受け取る変数 00060 //float rpm = 0; //rpmを格納するための変数 00061 //float t = 0; //時間計測のための変数 00062 //result exp2[(int)(15*1000/PID_RATE+1)] = {}; //モード2用の構造体 00063 //result exp3[(int)(15*1000/PID_RATE+1)] = {}; //モード3用の構造体 00064 result exp[(int)(15*1000/PID_RATE+1)] = {}; 00065 //メインループ 00066 while(1){ 00067 Init(); 00068 for(int i = 0; i <= 15*1000/PID_RATE; i++){ 00069 exp[i].t = 0; 00070 exp[i].rpm = 0; 00071 } 00072 SW(); //スタートスイッチの入力を待つ 00073 flag = GetDipValue(); //DIPスイッチの入力を見る 00074 DipLed(); //DIPスイッチのHLでLEDを光らせる 00075 Buzzer(1); //ブザーを鳴らす． 00076 switch (flag){ //実験モード切り替えのswitch文．ここにcase0~15を書き足せばモードが追加できる． 00077 case 0: //DIPスイッチの入力なしで何もしない 00078 break; 00079 case 1:{ //モード1はポテンショメータからの入力で直接PWMのデューティ比を変更する．約1秒ごとにPCに値を出力する．このモードは時間で終了せず永遠に続く．終了するにはmbedのresetボタンを押す． 00080 //初期化 00081 float vol = 0; //ポテンショメータからの入力値を受け取る変数 00082 float rpm = 0; //rpmを格納するための変数 00083 float t = 0; //時間計測のための変数 00084 int i1 = 0; 00085 encoder.reset(); 00086 timer.reset(); 00087 timer.start(); 00088 //ループ(抜け出せない) 00089 while(1){ 00090 vol = 1 - volume; 00091 pwm = vol; 00092 if(i1 >= 20){ 00093 t = timer.read(); 00094 rpm = (float)encoder.getPulses() / 2 / PULSE_PER_REVOLUTION * 60 / t * GEAR_RATIO; //減速比2 00095 pc.printf("Duty Ratio = %.3f , %6d RPM\n", vol, (int)rpm); 00096 encoder.reset(); 00097 i1 = 0; 00098 timer.reset(); 00099 } 00100 wait_ms(50); 00101 i1++; 00102 } 00103 break; 00104 } 00105 case 2:{ //モード2はモータの応答を調べる．デューティ比1でモータを回転させ，100msごとの経過時間とRPMをUSBメモリのcsvファイルに出力する．約15秒で終了する． 00106 //result exp2[(int)(15*1000/PID_RATE+1)] = {}; 00107 float dt = 0, tnow = 0, tpre = 0; 00108 //初期化 00109 exp[0].t = 0; 00110 exp[0].rpm = 0; 00111 encoder.reset(); 00112 timer.reset(); 00113 timer.start(); 00114 pwm = 1; //モータ回転開始!! 00115 //ループ(約15秒で終了) 00116 for(int i = 0; i < 15*1000/PID_RATE; i++){ //10msごとに時間とrpmの取得 00117 wait_ms(PID_RATE); 00118 tpre = tnow; 00119 tnow = timer.read(); 00120 exp[i+1].t = (unsigned short)(tnow * 1000); 00121 dt = tnow - tpre; 00122 exp[i+1].rpm = (short)((float)encoder.getPulses() / 2 / PULSE_PER_REVOLUTION * 60 / dt * GEAR_RATIO); //減速比2 00123 encoder.reset(); 00124 } 00125 for(int i = 100; i >= 0; i--){ //ゆるやかに減速 00126 pwm = (float)i / 100; 00127 wait_ms(20); 00128 } 00129 pwm = 0; //回転を止める 00130 wait(1); 00131 FILE * fp = fopen("/local/exp2.csv","w"); 00132 if(fp == NULL){ //ファイルオープンエラー 00133 Buzzer(-1); //エラーコード-1のブザーを鳴らす 00134 break; //終了 00135 } 00136 fprintf(fp,"Time , RPM\n"); 00137 for(int i = 0; i <= 15*1000/PID_RATE; i++){ 00138 fprintf(fp,"%f , %d\n", (float)exp[i].t / 1000.0, exp[i].rpm); //ファイルに実験データの書き込み 00139 } 00140 fclose(fp); 00141 Buzzer(4); //終了を知らせる 00142 break; 00143 } 00144 case 3:{ //モード3はモータの応答を調べる．PID制御をかけ，経過時間とRPMをUSBメモリのcsvファイルに出力する．約15秒で終了する． 00145 //result exp3[(int)(15*1000/PID_RATE+1)] = {}; 00146 float kp = 0, ti = 0, td = 0, max_rpm = 0, target_rpm = 0, reduction = 0, dt = 0, tnow = 0, tpre = 0, rpmnow = 0; 00147 //int pulse[15*1000/PID_RATE+2] = {}; 00148 //int j = 0; 00149 //float times = 0; 00150 encoder.reset(); 00151 timer.reset(); 00152 /* 00153 -----PID制御の係数設定ファイルpid.txtの書き方----- 00154 #kp 00155 1.0 00156 00157 #ki 00158 1.0 00159 00160 #kd 00161 0.0 00162 00163 #max_rpm 00164 10000 00165 エンコーダで読めるモータのrpmの最大値．これを超えないように制御を行う． 00166 00167 #target_rpm 00168 3000 00169 http://denki.nara-edu.ac.jp/~yabu/soft/header.htmlを参考にしました 00170 */ 00171 00172 /*FILE *fppid = fopen("/local/pid.txt","r"); 00173 if(fppid == NULL){ 00174 Buzzer(-2); 00175 break; 00176 }*/ 00177 //fscanf(fppid,"%f %f %f %f %f", &kp, &ki, &kd, &max_rpm, &target_rpm); 00178 //fclose(fppid); 00179 //find_header(fppid,"# kp"); //PID制御用の係数の読み込み 00180 //fscanf(fppid,"%f",&kp); 00181 //find_header(fppid,"# ki"); 00182 //fscanf(fppid,"%f",&ki); 00183 //find_header(fppid,"# kd"); 00184 //fscanf(fppid,"%f",&kd); 00185 //find_header(fppid,"# max_rpm"); 00186 //fscanf(fppid,"%f",&max_rpm); 00187 //find_header(fppid,"# target_rpm"); 00188 //fscanf(fppid,"%f",&target_rpm); 00189 //fclose(fppid); 00190 00191 kp = KP / KP_SCALE; 00192 ti = TI * TI_SCALE; 00193 td = TD; 00194 max_rpm = MAX_RPM; 00195 target_rpm = TARGET_RPM; 00196 00197 PID pid(kp, ti, td, PID_RATE); //PIDの設定 00198 pid.setInputLimits(0.0, max_rpm); 00199 pid.setOutputLimits(0.0, 1.0); 00200 pid.setMode(AUTO_MODE); 00201 pid.setSetPoint(target_rpm); 00202 00203 exp[0].t = 0; 00204 exp[0].rpm = 0; 00205 timer.start(); 00206 for(int i = 0; i < 15*1000/PID_RATE; i++){ //15秒間実行する 00207 tpre = tnow; 00208 tnow = timer.read(); 00209 exp[i+1].t = (unsigned short)(tnow * 1000); 00210 dt = tnow - tpre; 00211 rpmnow = (float)encoder.getPulses() / 2 / PULSE_PER_REVOLUTION * 60 / dt * GEAR_RATIO; 00212 exp[i+1].rpm = (short)rpmnow; 00213 pid.setProcessValue(rpmnow); 00214 pwm = pid.compute(); 00215 encoder.reset(); 00216 //pulse[i+1] = encoder.getPulses() / 2.0; 00217 //pid.setProcessValue((float)(pulse[i+1] - pulse[i]) / PULSE_PER_REVOLUTION * 60 / (timer.read() - times) * 2); 00218 //times = timer.read(); 00219 //if((i % (int)(100 / PID_RATE) == 0) && (i != 0)){ //100ms毎に時間とrpmを読み込む 00220 //j++; 00221 //exp3[j].t = timer.read(); 00222 //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; 00223 //} 00224 wait_ms(PID_RATE); 00225 } 00226 reduction = pwm.read(); 00227 for(int i = 0; i <= 100; i++){ 00228 pwm = pwm.read() - reduction / 100; 00229 wait_ms(20); 00230 } 00231 pwm = 0; 00232 wait(1); 00233 FILE *fp = fopen("/local/exp3.csv","w"); 00234 if(fp == NULL){ //ファイルオープンエラー 00235 Buzzer(-1); //エラーコード-1のブザーを鳴らす 00236 break; //終了 00237 } 00238 fprintf(fp,"kp = %.6f ki = %.6f kd = %.6f\n", (float)KP, (float)TI, (float)TD); 00239 fprintf(fp,"Time , RPM\n"); 00240 for(int i = 0; i <= 15*1000/PID_RATE; i++){ 00241 fprintf(fp,"%f , %d\n", (float)exp[i].t / 1000.0, exp[i].rpm); //ファイルに実験データの書き込み 00242 } 00243 fclose(fp); 00244 Buzzer(5); //終了を知らせる 00245 break; 00246 } 00247 default: //0と上記以外でなにもなし 00248 break; 00249 } 00250 } 00251 } 00252 00253 /**************************関数たち**************************/ 00254 int Init(){ //初期化関数 00255 pwm = 0; 00256 led1 = 0; 00257 led2 = 0; 00258 led3 = 0; 00259 led4 = 0; 00260 buzzer = 0; 00261 pc.baud(9600); 00262 encoder.reset(); 00263 timer.reset(); 00264 pwm.period_us(25); //40kHz 00265 DipLed(); 00266 return 0; 00267 } 00268 00269 int DipLed(){ //DIPスイッチの状態によってledを光らせる関数 00270 if(dip1 == 1) led1 = 1; else led1 = 0; //DIPスイッチの1がHならLEDを光らせる,Lなら光らせない 00271 if(dip2 == 1) led2 = 1; else led2 = 0; 00272 if(dip3 == 1) led3 = 1; else led3 = 0; 00273 if(dip4 == 1) led4 = 1; else led4 = 0; 00274 return 0; 00275 } 00276 00277 int GetDipValue(){ //DIPスイッチの値を取得し，値を返す関数 00278 int Dip1 = dip1, Dip2 = dip2, Dip3 = dip3, Dip4 = dip4; //DIPスイッチの値を取得 00279 if(Dip1 == 0){ 00280 if(Dip2 == 0){ 00281 if(Dip3 == 0){ 00282 if(Dip4 == 0) return 0; else return 1; //DIPスイッチが0000の場合0を返す．0001なら1 00283 }else{ 00284 if(Dip4 == 0) return 2; else return 3; 00285 } 00286 }else{ 00287 if(Dip3 == 0){ 00288 if(Dip4 == 0) return 4; else return 5; 00289 }else{ 00290 if(Dip4 == 0) return 6; else return 7; 00291 } 00292 } 00293 }else{ 00294 if(Dip2 == 0){ 00295 if(Dip3 == 0){ 00296 if(Dip4 == 0) return 8; else return 9; 00297 }else{ 00298 if(Dip4 == 0) return 10; else return 11; 00299 } 00300 }else{ 00301 if(Dip3 == 0){ 00302 if(Dip4 == 0) return 12; else return 13; 00303 }else{ 00304 if(Dip4 == 0) return 14; else return 15; 00305 } 00306 } 00307 } 00308 } 00309 00310 int SW(){ //スタートスイッチ用関数，押して離したらスタート 00311 int i = 0, j = 0; 00312 while(i < 3){ //チャタリング除去，15msにわたってスタートスイッチが押されていればbreak 00313 if(startsw == 1) i++; 00314 else i = 0; 00315 DipLed(); 00316 wait_ms(5); 00317 } 00318 while(j < 3){ //上に同じ，スタートスイッチが離されたことを検知 00319 if(startsw == 0) j++; 00320 else j = 0; 00321 DipLed(); 00322 wait_ms(5); 00323 } 00324 return 0; 00325 } 00326 00327 int Buzzer(int buzvar){ //電子ブザーを鳴らす関数 00328 switch (buzvar){ 00329 /**************エラーを知らせるbeep**************/ 00330 case -3: //error * - - 00331 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00332 buzzer = 1; wait(0.3); buzzer = 0; wait(0.3); 00333 buzzer = 1; wait(0.3); buzzer = 0; wait(0.3); 00334 break; 00335 case -2: //error * - - - 00336 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00337 buzzer = 1; wait(0.3); buzzer = 0; wait(0.3); 00338 buzzer = 1; wait(0.3); buzzer = 0; wait(0.3); 00339 buzzer = 1; wait(0.3); buzzer = 0; wait(0.3); 00340 break; 00341 case -1: //error * - - - - 00342 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00343 buzzer = 1; wait(0.3); buzzer = 0; wait(0.3); 00344 buzzer = 1; wait(0.3); buzzer = 0; wait(0.3); 00345 buzzer = 1; wait(0.3); buzzer = 0; wait(0.3); 00346 buzzer = 1; wait(0.3); buzzer = 0; wait(0.3); 00347 break; 00348 /**************エラーここまで**************/ 00349 case 0: //サウンドなし 00350 buzzer = 0; 00351 break; 00352 /**************状態を知らせるためのbeep**************/ 00353 case 1: // *(短) 00354 buzzer = 1; wait(0.1); buzzer = 0; 00355 break; 00356 case 2: // * * 00357 buzzer = 1; wait(0.1); buzzer = 0; wait(0.05); 00358 buzzer = 1; wait(0.1); buzzer = 0; 00359 break; 00360 case 3: // -(長) 00361 buzzer = 1; wait(0.3); buzzer = 0; 00362 break; 00363 case 4: // - - 00364 buzzer = 1; wait(0.3); buzzer = 0; wait(0.3); 00365 buzzer = 1; wait(0.3); buzzer = 0; 00366 break; 00367 case 5: // --- 00368 buzzer = 1; wait(0.9); buzzer = 0; 00369 break; 00370 case 6: // * * * * * * * * * * 00371 for(int i = 0; i < 3; i++){ 00372 for(int j = 0; j < 3; j++){ 00373 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00374 } 00375 wait(0.2); 00376 } 00377 buzzer = 1; wait(0.1); buzzer = 0; 00378 break; 00379 case 7: // **-* ** -* ** *** **** "finish" 00380 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00381 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00382 buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 00383 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00384 wait(0.2); 00385 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00386 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00387 wait(0.2); 00388 buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 00389 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00390 wait(0.2); 00391 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00392 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00393 wait(0.2); 00394 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00395 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00396 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00397 wait(0.2); 00398 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00399 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00400 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00401 buzzer = 1; wait(0.1); buzzer = 0; 00402 break; 00403 case 8: // *-*** -*- --* "オワリ" 00404 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00405 buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 00406 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00407 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00408 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00409 wait(0.2); 00410 buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 00411 buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); 00412 buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 00413 wait(0.2); 00414 buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 00415 buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); 00416 buzzer = 1; wait(0.1); buzzer = 0; 00417 break; 00418 /**************状態を知らせるためのbeepここまで**************/ 00419 default: //no sound 00420 buzzer = 0; 00421 break; 00422 } 00423 return 0; 00424 } 00425 00426 00427 int find_header(FILE *fp, char keyword[]) 00428 /* 00429 入力 fp ファイルポインタ 00430 keyword キーワード 00431 戻り値 1 : ヘッダを見つけた 0 ヘッダが見つからなかった 00432 */ 00433 { 00434 char char_buf[256]; 00435 char keyword_buf[256]; 00436 char *condition; 00437 int len,i; 00438 00439 strcpy(keyword_buf,keyword); 00440 rm_right_space(keyword_buf); /* 右側の空白を取り除く */ 00441 00442 rewind(fp); 00443 for(;;){ 00444 condition = fgets(char_buf,BUFSIZ,fp); 00445 if ( condition == NULL ) break; 00446 rm_right_space(char_buf); /* 右側の空白と改行記号を取り除く */ 00447 len = strlen(char_buf); 00448 if ( len == 0 ) continue; /* この文はなくてもよい */ 00449 for ( i = 0 ; i < len ; i++ ){ /* 左側の空白を取り除く */ 00450 if ( char_buf[i] != ' ' ) break; 00451 } 00452 if ( strcmp(&char_buf[i],keyword_buf)==0){ 00453 return(1); 00454 break; 00455 } 00456 } 00457 fprintf(stderr,"cannot find header. key word : |%s|\n",keyword); 00458 Buzzer(-3); 00459 return(0); 00460 } 00461 00462 /* 文字列の右側の空白を削除する 改行記号がある場合はそれも除去する */ 00463 00464 void rm_right_space(char str[]) 00465 { 00466 int i; 00467 00468 i = strlen(str)-1; 00469 if ( str[i] == '\n' ) i--; 00470 00471 while(i>=0){ 00472 if ( str[i] != ' ') break; 00473 i--; 00474 } 00475 str[i+1] = '\0'; 00476 }
Generated on Wed Jul 20 2022 06:34:58 by
1.7.2