機械工学実験1作業用
Dependencies: PID QEI USBHost mbed
main.cpp@1:36385b59d183, 2015-04-20 (annotated)
- Committer:
- neoqased
- Date:
- Mon Apr 20 03:34:10 2015 +0000
- Revision:
- 1:36385b59d183
- Parent:
- 0:d7629adcea6d
kikaikougakujikken
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
neoqased | 0:d7629adcea6d | 1 | //※重要※ 日本語コメントアウトはmbedのBeta-japaneseに登録すると使えるようになる(2014年現在) |
neoqased | 0:d7629adcea6d | 2 | //めんどくさいからファイル分割しないよ.熱意がある人はやって頂戴 by小林2014-9-12 |
neoqased | 0:d7629adcea6d | 3 | #include "mbed.h" //updateのマークが出ていてもupdateしないように.なんか仕様が変わってるっぽいからエラー吐くようになる |
neoqased | 0:d7629adcea6d | 4 | #include "QEI.h" |
neoqased | 0:d7629adcea6d | 5 | //#include "USBHostMSD.h" //updateのマークが出ていてもupdateしないように.なんか仕様が変わってるっぽいからエラー吐くようになる |
neoqased | 0:d7629adcea6d | 6 | #include "PID.h" |
neoqased | 0:d7629adcea6d | 7 | #include "setting.h" |
neoqased | 0:d7629adcea6d | 8 | |
neoqased | 0:d7629adcea6d | 9 | //1回転あたりのパルス |
neoqased | 0:d7629adcea6d | 10 | #define PULSE_PER_REVOLUTION 200 |
neoqased | 1:36385b59d183 | 11 | //PIDのウェイト ミリ秒(制御周期) |
neoqased | 0:d7629adcea6d | 12 | #define PID_RATE 10 |
neoqased | 1:36385b59d183 | 13 | //ギア比 |
neoqased | 1:36385b59d183 | 14 | #define GEAR_RATIO 2 |
neoqased | 1:36385b59d183 | 15 | |
neoqased | 1:36385b59d183 | 16 | #define KP_SCALE 10 |
neoqased | 1:36385b59d183 | 17 | #define TI_SCALE 20 |
neoqased | 0:d7629adcea6d | 18 | |
neoqased | 0:d7629adcea6d | 19 | /**************************入出力ポート設定**************************/ |
neoqased | 0:d7629adcea6d | 20 | DigitalOut led1(LED1); //mbed上LED出力4つ |
neoqased | 0:d7629adcea6d | 21 | DigitalOut led2(LED2); |
neoqased | 0:d7629adcea6d | 22 | DigitalOut led3(LED3); |
neoqased | 0:d7629adcea6d | 23 | DigitalOut led4(LED4); |
neoqased | 0:d7629adcea6d | 24 | DigitalIn dip1(p5); //DIPスイッチの入力4つ |
neoqased | 0:d7629adcea6d | 25 | DigitalIn dip2(p6); |
neoqased | 0:d7629adcea6d | 26 | DigitalIn dip3(p7); |
neoqased | 0:d7629adcea6d | 27 | DigitalIn dip4(p8); |
neoqased | 0:d7629adcea6d | 28 | DigitalIn startsw(p9); //スタートスイッチ入力 |
neoqased | 0:d7629adcea6d | 29 | DigitalOut buzzer(p18); //電子ブザー用デジタル出力 |
neoqased | 0:d7629adcea6d | 30 | AnalogIn volume(p20); //ポテンショメータ入力(アナログ) |
neoqased | 0:d7629adcea6d | 31 | PwmOut pwm(p25); //モータへのPWM出力 |
neoqased | 0:d7629adcea6d | 32 | |
neoqased | 0:d7629adcea6d | 33 | Serial pc(USBTX, USBRX); //PCとのシリアル通信設定 |
neoqased | 0:d7629adcea6d | 34 | QEI encoder(p30, p29, NC, PULSE_PER_REVOLUTION, QEI::X2_ENCODING); //QEIの設定. |
neoqased | 0:d7629adcea6d | 35 | Timer timer; //タイマー |
neoqased | 0:d7629adcea6d | 36 | |
neoqased | 0:d7629adcea6d | 37 | typedef struct{ //時間tとその時のrpmを記録するための構造体 |
neoqased | 1:36385b59d183 | 38 | unsigned short t; |
neoqased | 1:36385b59d183 | 39 | short rpm; |
neoqased | 0:d7629adcea6d | 40 | } result; |
neoqased | 0:d7629adcea6d | 41 | |
neoqased | 0:d7629adcea6d | 42 | /**************************プロトタイプ宣言**************************/ |
neoqased | 0:d7629adcea6d | 43 | int Init(); |
neoqased | 0:d7629adcea6d | 44 | int DipLed(); |
neoqased | 0:d7629adcea6d | 45 | int GetDipValue(); |
neoqased | 0:d7629adcea6d | 46 | int SW(); |
neoqased | 0:d7629adcea6d | 47 | int Buzzer(int buzvar); |
neoqased | 0:d7629adcea6d | 48 | int find_header(FILE *fp, char keyword[]); |
neoqased | 0:d7629adcea6d | 49 | void rm_right_space(char str[]); |
neoqased | 0:d7629adcea6d | 50 | |
neoqased | 0:d7629adcea6d | 51 | /**************************main文**************************/ |
neoqased | 0:d7629adcea6d | 52 | int main() { |
neoqased | 0:d7629adcea6d | 53 | //初期化 |
neoqased | 0:d7629adcea6d | 54 | Buzzer(2); |
neoqased | 0:d7629adcea6d | 55 | //USBHostMSD msd("usb"); |
neoqased | 0:d7629adcea6d | 56 | LocalFileSystem local("local"); |
neoqased | 0:d7629adcea6d | 57 | Init(); //初期化関数 |
neoqased | 0:d7629adcea6d | 58 | int flag = 0; //フラグ用変数 |
neoqased | 0:d7629adcea6d | 59 | //float vol = 0; //ポテンショメータからの入力値を受け取る変数 |
neoqased | 0:d7629adcea6d | 60 | //float rpm = 0; //rpmを格納するための変数 |
neoqased | 0:d7629adcea6d | 61 | //float t = 0; //時間計測のための変数 |
neoqased | 0:d7629adcea6d | 62 | //result exp2[(int)(15*1000/PID_RATE+1)] = {}; //モード2用の構造体 |
neoqased | 0:d7629adcea6d | 63 | //result exp3[(int)(15*1000/PID_RATE+1)] = {}; //モード3用の構造体 |
neoqased | 0:d7629adcea6d | 64 | result exp[(int)(15*1000/PID_RATE+1)] = {}; |
neoqased | 0:d7629adcea6d | 65 | //メインループ |
neoqased | 0:d7629adcea6d | 66 | while(1){ |
neoqased | 0:d7629adcea6d | 67 | Init(); |
neoqased | 0:d7629adcea6d | 68 | for(int i = 0; i <= 15*1000/PID_RATE; i++){ |
neoqased | 0:d7629adcea6d | 69 | exp[i].t = 0; |
neoqased | 0:d7629adcea6d | 70 | exp[i].rpm = 0; |
neoqased | 0:d7629adcea6d | 71 | } |
neoqased | 0:d7629adcea6d | 72 | SW(); //スタートスイッチの入力を待つ |
neoqased | 0:d7629adcea6d | 73 | flag = GetDipValue(); //DIPスイッチの入力を見る |
neoqased | 0:d7629adcea6d | 74 | DipLed(); //DIPスイッチのHLでLEDを光らせる |
neoqased | 0:d7629adcea6d | 75 | Buzzer(1); //ブザーを鳴らす. |
neoqased | 0:d7629adcea6d | 76 | switch (flag){ //実験モード切り替えのswitch文.ここにcase0~15を書き足せばモードが追加できる. |
neoqased | 0:d7629adcea6d | 77 | case 0: //DIPスイッチの入力なしで何もしない |
neoqased | 0:d7629adcea6d | 78 | break; |
neoqased | 0:d7629adcea6d | 79 | case 1:{ //モード1はポテンショメータからの入力で直接PWMのデューティ比を変更する.約1秒ごとにPCに値を出力する.このモードは時間で終了せず永遠に続く.終了するにはmbedのresetボタンを押す. |
neoqased | 0:d7629adcea6d | 80 | //初期化 |
neoqased | 0:d7629adcea6d | 81 | float vol = 0; //ポテンショメータからの入力値を受け取る変数 |
neoqased | 0:d7629adcea6d | 82 | float rpm = 0; //rpmを格納するための変数 |
neoqased | 0:d7629adcea6d | 83 | float t = 0; //時間計測のための変数 |
neoqased | 0:d7629adcea6d | 84 | int i1 = 0; |
neoqased | 0:d7629adcea6d | 85 | encoder.reset(); |
neoqased | 0:d7629adcea6d | 86 | timer.reset(); |
neoqased | 0:d7629adcea6d | 87 | timer.start(); |
neoqased | 0:d7629adcea6d | 88 | //ループ(抜け出せない) |
neoqased | 0:d7629adcea6d | 89 | while(1){ |
neoqased | 0:d7629adcea6d | 90 | vol = 1 - volume; |
neoqased | 0:d7629adcea6d | 91 | pwm = vol; |
neoqased | 0:d7629adcea6d | 92 | if(i1 >= 20){ |
neoqased | 0:d7629adcea6d | 93 | t = timer.read(); |
neoqased | 1:36385b59d183 | 94 | rpm = (float)encoder.getPulses() / 2 / PULSE_PER_REVOLUTION * 60 / t * GEAR_RATIO; //減速比2 |
neoqased | 0:d7629adcea6d | 95 | pc.printf("Duty Ratio = %.3f , %6d RPM\n", vol, (int)rpm); |
neoqased | 0:d7629adcea6d | 96 | encoder.reset(); |
neoqased | 0:d7629adcea6d | 97 | i1 = 0; |
neoqased | 0:d7629adcea6d | 98 | timer.reset(); |
neoqased | 0:d7629adcea6d | 99 | } |
neoqased | 0:d7629adcea6d | 100 | wait_ms(50); |
neoqased | 0:d7629adcea6d | 101 | i1++; |
neoqased | 0:d7629adcea6d | 102 | } |
neoqased | 0:d7629adcea6d | 103 | break; |
neoqased | 0:d7629adcea6d | 104 | } |
neoqased | 0:d7629adcea6d | 105 | case 2:{ //モード2はモータの応答を調べる.デューティ比1でモータを回転させ,100msごとの経過時間とRPMをUSBメモリのcsvファイルに出力する.約15秒で終了する. |
neoqased | 0:d7629adcea6d | 106 | //result exp2[(int)(15*1000/PID_RATE+1)] = {}; |
neoqased | 1:36385b59d183 | 107 | float dt = 0, tnow = 0, tpre = 0; |
neoqased | 0:d7629adcea6d | 108 | //初期化 |
neoqased | 0:d7629adcea6d | 109 | exp[0].t = 0; |
neoqased | 0:d7629adcea6d | 110 | exp[0].rpm = 0; |
neoqased | 0:d7629adcea6d | 111 | encoder.reset(); |
neoqased | 0:d7629adcea6d | 112 | timer.reset(); |
neoqased | 0:d7629adcea6d | 113 | timer.start(); |
neoqased | 0:d7629adcea6d | 114 | pwm = 1; //モータ回転開始!! |
neoqased | 0:d7629adcea6d | 115 | //ループ(約15秒で終了) |
neoqased | 0:d7629adcea6d | 116 | for(int i = 0; i < 15*1000/PID_RATE; i++){ //10msごとに時間とrpmの取得 |
neoqased | 0:d7629adcea6d | 117 | wait_ms(PID_RATE); |
neoqased | 1:36385b59d183 | 118 | tpre = tnow; |
neoqased | 1:36385b59d183 | 119 | tnow = timer.read(); |
neoqased | 1:36385b59d183 | 120 | exp[i+1].t = (unsigned short)(tnow * 1000); |
neoqased | 1:36385b59d183 | 121 | dt = tnow - tpre; |
neoqased | 1:36385b59d183 | 122 | exp[i+1].rpm = (short)((float)encoder.getPulses() / 2 / PULSE_PER_REVOLUTION * 60 / dt * GEAR_RATIO); //減速比2 |
neoqased | 0:d7629adcea6d | 123 | encoder.reset(); |
neoqased | 0:d7629adcea6d | 124 | } |
neoqased | 0:d7629adcea6d | 125 | for(int i = 100; i >= 0; i--){ //ゆるやかに減速 |
neoqased | 0:d7629adcea6d | 126 | pwm = (float)i / 100; |
neoqased | 0:d7629adcea6d | 127 | wait_ms(20); |
neoqased | 0:d7629adcea6d | 128 | } |
neoqased | 0:d7629adcea6d | 129 | pwm = 0; //回転を止める |
neoqased | 0:d7629adcea6d | 130 | wait(1); |
neoqased | 0:d7629adcea6d | 131 | FILE * fp = fopen("/local/exp2.csv","w"); |
neoqased | 0:d7629adcea6d | 132 | if(fp == NULL){ //ファイルオープンエラー |
neoqased | 0:d7629adcea6d | 133 | Buzzer(-1); //エラーコード-1のブザーを鳴らす |
neoqased | 0:d7629adcea6d | 134 | break; //終了 |
neoqased | 0:d7629adcea6d | 135 | } |
neoqased | 0:d7629adcea6d | 136 | fprintf(fp,"Time , RPM\n"); |
neoqased | 0:d7629adcea6d | 137 | for(int i = 0; i <= 15*1000/PID_RATE; i++){ |
neoqased | 1:36385b59d183 | 138 | fprintf(fp,"%f , %d\n", (float)exp[i].t / 1000.0, exp[i].rpm); //ファイルに実験データの書き込み |
neoqased | 0:d7629adcea6d | 139 | } |
neoqased | 0:d7629adcea6d | 140 | fclose(fp); |
neoqased | 0:d7629adcea6d | 141 | Buzzer(4); //終了を知らせる |
neoqased | 0:d7629adcea6d | 142 | break; |
neoqased | 0:d7629adcea6d | 143 | } |
neoqased | 0:d7629adcea6d | 144 | case 3:{ //モード3はモータの応答を調べる.PID制御をかけ,経過時間とRPMをUSBメモリのcsvファイルに出力する.約15秒で終了する. |
neoqased | 0:d7629adcea6d | 145 | //result exp3[(int)(15*1000/PID_RATE+1)] = {}; |
neoqased | 1:36385b59d183 | 146 | float kp = 0, ti = 0, td = 0, max_rpm = 0, target_rpm = 0, reduction = 0, dt = 0, tnow = 0, tpre = 0, rpmnow = 0; |
neoqased | 0:d7629adcea6d | 147 | //int pulse[15*1000/PID_RATE+2] = {}; |
neoqased | 0:d7629adcea6d | 148 | //int j = 0; |
neoqased | 0:d7629adcea6d | 149 | //float times = 0; |
neoqased | 0:d7629adcea6d | 150 | encoder.reset(); |
neoqased | 0:d7629adcea6d | 151 | timer.reset(); |
neoqased | 0:d7629adcea6d | 152 | /* |
neoqased | 0:d7629adcea6d | 153 | -----PID制御の係数設定ファイルpid.txtの書き方----- |
neoqased | 0:d7629adcea6d | 154 | #kp |
neoqased | 0:d7629adcea6d | 155 | 1.0 |
neoqased | 0:d7629adcea6d | 156 | |
neoqased | 0:d7629adcea6d | 157 | #ki |
neoqased | 0:d7629adcea6d | 158 | 1.0 |
neoqased | 0:d7629adcea6d | 159 | |
neoqased | 0:d7629adcea6d | 160 | #kd |
neoqased | 0:d7629adcea6d | 161 | 0.0 |
neoqased | 0:d7629adcea6d | 162 | |
neoqased | 0:d7629adcea6d | 163 | #max_rpm |
neoqased | 0:d7629adcea6d | 164 | 10000 |
neoqased | 0:d7629adcea6d | 165 | エンコーダで読めるモータのrpmの最大値.これを超えないように制御を行う. |
neoqased | 0:d7629adcea6d | 166 | |
neoqased | 0:d7629adcea6d | 167 | #target_rpm |
neoqased | 0:d7629adcea6d | 168 | 3000 |
neoqased | 0:d7629adcea6d | 169 | http://denki.nara-edu.ac.jp/~yabu/soft/header.htmlを参考にしました |
neoqased | 0:d7629adcea6d | 170 | */ |
neoqased | 0:d7629adcea6d | 171 | |
neoqased | 0:d7629adcea6d | 172 | /*FILE *fppid = fopen("/local/pid.txt","r"); |
neoqased | 0:d7629adcea6d | 173 | if(fppid == NULL){ |
neoqased | 0:d7629adcea6d | 174 | Buzzer(-2); |
neoqased | 0:d7629adcea6d | 175 | break; |
neoqased | 0:d7629adcea6d | 176 | }*/ |
neoqased | 0:d7629adcea6d | 177 | //fscanf(fppid,"%f %f %f %f %f", &kp, &ki, &kd, &max_rpm, &target_rpm); |
neoqased | 0:d7629adcea6d | 178 | //fclose(fppid); |
neoqased | 0:d7629adcea6d | 179 | //find_header(fppid,"# kp"); //PID制御用の係数の読み込み |
neoqased | 0:d7629adcea6d | 180 | //fscanf(fppid,"%f",&kp); |
neoqased | 0:d7629adcea6d | 181 | //find_header(fppid,"# ki"); |
neoqased | 0:d7629adcea6d | 182 | //fscanf(fppid,"%f",&ki); |
neoqased | 0:d7629adcea6d | 183 | //find_header(fppid,"# kd"); |
neoqased | 0:d7629adcea6d | 184 | //fscanf(fppid,"%f",&kd); |
neoqased | 0:d7629adcea6d | 185 | //find_header(fppid,"# max_rpm"); |
neoqased | 0:d7629adcea6d | 186 | //fscanf(fppid,"%f",&max_rpm); |
neoqased | 0:d7629adcea6d | 187 | //find_header(fppid,"# target_rpm"); |
neoqased | 0:d7629adcea6d | 188 | //fscanf(fppid,"%f",&target_rpm); |
neoqased | 0:d7629adcea6d | 189 | //fclose(fppid); |
neoqased | 0:d7629adcea6d | 190 | |
neoqased | 1:36385b59d183 | 191 | kp = KP / KP_SCALE; |
neoqased | 1:36385b59d183 | 192 | ti = TI * TI_SCALE; |
neoqased | 1:36385b59d183 | 193 | td = TD; |
neoqased | 0:d7629adcea6d | 194 | max_rpm = MAX_RPM; |
neoqased | 0:d7629adcea6d | 195 | target_rpm = TARGET_RPM; |
neoqased | 0:d7629adcea6d | 196 | |
neoqased | 1:36385b59d183 | 197 | PID pid(kp, ti, td, PID_RATE); //PIDの設定 |
neoqased | 0:d7629adcea6d | 198 | pid.setInputLimits(0.0, max_rpm); |
neoqased | 0:d7629adcea6d | 199 | pid.setOutputLimits(0.0, 1.0); |
neoqased | 0:d7629adcea6d | 200 | pid.setMode(AUTO_MODE); |
neoqased | 0:d7629adcea6d | 201 | pid.setSetPoint(target_rpm); |
neoqased | 0:d7629adcea6d | 202 | |
neoqased | 0:d7629adcea6d | 203 | exp[0].t = 0; |
neoqased | 0:d7629adcea6d | 204 | exp[0].rpm = 0; |
neoqased | 0:d7629adcea6d | 205 | timer.start(); |
neoqased | 0:d7629adcea6d | 206 | for(int i = 0; i < 15*1000/PID_RATE; i++){ //15秒間実行する |
neoqased | 1:36385b59d183 | 207 | tpre = tnow; |
neoqased | 1:36385b59d183 | 208 | tnow = timer.read(); |
neoqased | 1:36385b59d183 | 209 | exp[i+1].t = (unsigned short)(tnow * 1000); |
neoqased | 1:36385b59d183 | 210 | dt = tnow - tpre; |
neoqased | 1:36385b59d183 | 211 | rpmnow = (float)encoder.getPulses() / 2 / PULSE_PER_REVOLUTION * 60 / dt * GEAR_RATIO; |
neoqased | 1:36385b59d183 | 212 | exp[i+1].rpm = (short)rpmnow; |
neoqased | 1:36385b59d183 | 213 | pid.setProcessValue(rpmnow); |
neoqased | 0:d7629adcea6d | 214 | pwm = pid.compute(); |
neoqased | 0:d7629adcea6d | 215 | encoder.reset(); |
neoqased | 0:d7629adcea6d | 216 | //pulse[i+1] = encoder.getPulses() / 2.0; |
neoqased | 0:d7629adcea6d | 217 | //pid.setProcessValue((float)(pulse[i+1] - pulse[i]) / PULSE_PER_REVOLUTION * 60 / (timer.read() - times) * 2); |
neoqased | 0:d7629adcea6d | 218 | //times = timer.read(); |
neoqased | 0:d7629adcea6d | 219 | //if((i % (int)(100 / PID_RATE) == 0) && (i != 0)){ //100ms毎に時間とrpmを読み込む |
neoqased | 0:d7629adcea6d | 220 | //j++; |
neoqased | 0:d7629adcea6d | 221 | //exp3[j].t = timer.read(); |
neoqased | 0:d7629adcea6d | 222 | //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; |
neoqased | 0:d7629adcea6d | 223 | //} |
neoqased | 0:d7629adcea6d | 224 | wait_ms(PID_RATE); |
neoqased | 0:d7629adcea6d | 225 | } |
neoqased | 0:d7629adcea6d | 226 | reduction = pwm.read(); |
neoqased | 0:d7629adcea6d | 227 | for(int i = 0; i <= 100; i++){ |
neoqased | 0:d7629adcea6d | 228 | pwm = pwm.read() - reduction / 100; |
neoqased | 0:d7629adcea6d | 229 | wait_ms(20); |
neoqased | 0:d7629adcea6d | 230 | } |
neoqased | 0:d7629adcea6d | 231 | pwm = 0; |
neoqased | 0:d7629adcea6d | 232 | wait(1); |
neoqased | 0:d7629adcea6d | 233 | FILE *fp = fopen("/local/exp3.csv","w"); |
neoqased | 0:d7629adcea6d | 234 | if(fp == NULL){ //ファイルオープンエラー |
neoqased | 0:d7629adcea6d | 235 | Buzzer(-1); //エラーコード-1のブザーを鳴らす |
neoqased | 0:d7629adcea6d | 236 | break; //終了 |
neoqased | 0:d7629adcea6d | 237 | } |
neoqased | 1:36385b59d183 | 238 | fprintf(fp,"kp = %.6f ki = %.6f kd = %.6f\n", (float)KP, (float)TI, (float)TD); |
neoqased | 0:d7629adcea6d | 239 | fprintf(fp,"Time , RPM\n"); |
neoqased | 0:d7629adcea6d | 240 | for(int i = 0; i <= 15*1000/PID_RATE; i++){ |
neoqased | 1:36385b59d183 | 241 | fprintf(fp,"%f , %d\n", (float)exp[i].t / 1000.0, exp[i].rpm); //ファイルに実験データの書き込み |
neoqased | 0:d7629adcea6d | 242 | } |
neoqased | 0:d7629adcea6d | 243 | fclose(fp); |
neoqased | 0:d7629adcea6d | 244 | Buzzer(5); //終了を知らせる |
neoqased | 0:d7629adcea6d | 245 | break; |
neoqased | 0:d7629adcea6d | 246 | } |
neoqased | 0:d7629adcea6d | 247 | default: //0と上記以外でなにもなし |
neoqased | 0:d7629adcea6d | 248 | break; |
neoqased | 0:d7629adcea6d | 249 | } |
neoqased | 0:d7629adcea6d | 250 | } |
neoqased | 0:d7629adcea6d | 251 | } |
neoqased | 0:d7629adcea6d | 252 | |
neoqased | 0:d7629adcea6d | 253 | /**************************関数たち**************************/ |
neoqased | 0:d7629adcea6d | 254 | int Init(){ //初期化関数 |
neoqased | 0:d7629adcea6d | 255 | pwm = 0; |
neoqased | 0:d7629adcea6d | 256 | led1 = 0; |
neoqased | 0:d7629adcea6d | 257 | led2 = 0; |
neoqased | 0:d7629adcea6d | 258 | led3 = 0; |
neoqased | 0:d7629adcea6d | 259 | led4 = 0; |
neoqased | 0:d7629adcea6d | 260 | buzzer = 0; |
neoqased | 0:d7629adcea6d | 261 | pc.baud(9600); |
neoqased | 0:d7629adcea6d | 262 | encoder.reset(); |
neoqased | 0:d7629adcea6d | 263 | timer.reset(); |
neoqased | 0:d7629adcea6d | 264 | pwm.period_us(25); //40kHz |
neoqased | 0:d7629adcea6d | 265 | DipLed(); |
neoqased | 0:d7629adcea6d | 266 | return 0; |
neoqased | 0:d7629adcea6d | 267 | } |
neoqased | 0:d7629adcea6d | 268 | |
neoqased | 0:d7629adcea6d | 269 | int DipLed(){ //DIPスイッチの状態によってledを光らせる関数 |
neoqased | 0:d7629adcea6d | 270 | if(dip1 == 1) led1 = 1; else led1 = 0; //DIPスイッチの1がHならLEDを光らせる,Lなら光らせない |
neoqased | 0:d7629adcea6d | 271 | if(dip2 == 1) led2 = 1; else led2 = 0; |
neoqased | 0:d7629adcea6d | 272 | if(dip3 == 1) led3 = 1; else led3 = 0; |
neoqased | 0:d7629adcea6d | 273 | if(dip4 == 1) led4 = 1; else led4 = 0; |
neoqased | 0:d7629adcea6d | 274 | return 0; |
neoqased | 0:d7629adcea6d | 275 | } |
neoqased | 0:d7629adcea6d | 276 | |
neoqased | 0:d7629adcea6d | 277 | int GetDipValue(){ //DIPスイッチの値を取得し,値を返す関数 |
neoqased | 0:d7629adcea6d | 278 | int Dip1 = dip1, Dip2 = dip2, Dip3 = dip3, Dip4 = dip4; //DIPスイッチの値を取得 |
neoqased | 0:d7629adcea6d | 279 | if(Dip1 == 0){ |
neoqased | 0:d7629adcea6d | 280 | if(Dip2 == 0){ |
neoqased | 0:d7629adcea6d | 281 | if(Dip3 == 0){ |
neoqased | 0:d7629adcea6d | 282 | if(Dip4 == 0) return 0; else return 1; //DIPスイッチが0000の場合0を返す.0001なら1 |
neoqased | 0:d7629adcea6d | 283 | }else{ |
neoqased | 0:d7629adcea6d | 284 | if(Dip4 == 0) return 2; else return 3; |
neoqased | 0:d7629adcea6d | 285 | } |
neoqased | 0:d7629adcea6d | 286 | }else{ |
neoqased | 0:d7629adcea6d | 287 | if(Dip3 == 0){ |
neoqased | 0:d7629adcea6d | 288 | if(Dip4 == 0) return 4; else return 5; |
neoqased | 0:d7629adcea6d | 289 | }else{ |
neoqased | 0:d7629adcea6d | 290 | if(Dip4 == 0) return 6; else return 7; |
neoqased | 0:d7629adcea6d | 291 | } |
neoqased | 0:d7629adcea6d | 292 | } |
neoqased | 0:d7629adcea6d | 293 | }else{ |
neoqased | 0:d7629adcea6d | 294 | if(Dip2 == 0){ |
neoqased | 0:d7629adcea6d | 295 | if(Dip3 == 0){ |
neoqased | 0:d7629adcea6d | 296 | if(Dip4 == 0) return 8; else return 9; |
neoqased | 0:d7629adcea6d | 297 | }else{ |
neoqased | 0:d7629adcea6d | 298 | if(Dip4 == 0) return 10; else return 11; |
neoqased | 0:d7629adcea6d | 299 | } |
neoqased | 0:d7629adcea6d | 300 | }else{ |
neoqased | 0:d7629adcea6d | 301 | if(Dip3 == 0){ |
neoqased | 0:d7629adcea6d | 302 | if(Dip4 == 0) return 12; else return 13; |
neoqased | 0:d7629adcea6d | 303 | }else{ |
neoqased | 0:d7629adcea6d | 304 | if(Dip4 == 0) return 14; else return 15; |
neoqased | 0:d7629adcea6d | 305 | } |
neoqased | 0:d7629adcea6d | 306 | } |
neoqased | 0:d7629adcea6d | 307 | } |
neoqased | 0:d7629adcea6d | 308 | } |
neoqased | 0:d7629adcea6d | 309 | |
neoqased | 0:d7629adcea6d | 310 | int SW(){ //スタートスイッチ用関数,押して離したらスタート |
neoqased | 0:d7629adcea6d | 311 | int i = 0, j = 0; |
neoqased | 0:d7629adcea6d | 312 | while(i < 3){ //チャタリング除去,15msにわたってスタートスイッチが押されていればbreak |
neoqased | 0:d7629adcea6d | 313 | if(startsw == 1) i++; |
neoqased | 0:d7629adcea6d | 314 | else i = 0; |
neoqased | 0:d7629adcea6d | 315 | DipLed(); |
neoqased | 0:d7629adcea6d | 316 | wait_ms(5); |
neoqased | 0:d7629adcea6d | 317 | } |
neoqased | 0:d7629adcea6d | 318 | while(j < 3){ //上に同じ,スタートスイッチが離されたことを検知 |
neoqased | 0:d7629adcea6d | 319 | if(startsw == 0) j++; |
neoqased | 0:d7629adcea6d | 320 | else j = 0; |
neoqased | 0:d7629adcea6d | 321 | DipLed(); |
neoqased | 0:d7629adcea6d | 322 | wait_ms(5); |
neoqased | 0:d7629adcea6d | 323 | } |
neoqased | 0:d7629adcea6d | 324 | return 0; |
neoqased | 0:d7629adcea6d | 325 | } |
neoqased | 0:d7629adcea6d | 326 | |
neoqased | 0:d7629adcea6d | 327 | int Buzzer(int buzvar){ //電子ブザーを鳴らす関数 |
neoqased | 0:d7629adcea6d | 328 | switch (buzvar){ |
neoqased | 0:d7629adcea6d | 329 | /**************エラーを知らせるbeep**************/ |
neoqased | 0:d7629adcea6d | 330 | case -3: //error * - - |
neoqased | 0:d7629adcea6d | 331 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 332 | buzzer = 1; wait(0.3); buzzer = 0; wait(0.3); |
neoqased | 0:d7629adcea6d | 333 | buzzer = 1; wait(0.3); buzzer = 0; wait(0.3); |
neoqased | 0:d7629adcea6d | 334 | break; |
neoqased | 0:d7629adcea6d | 335 | case -2: //error * - - - |
neoqased | 0:d7629adcea6d | 336 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 337 | buzzer = 1; wait(0.3); buzzer = 0; wait(0.3); |
neoqased | 0:d7629adcea6d | 338 | buzzer = 1; wait(0.3); buzzer = 0; wait(0.3); |
neoqased | 0:d7629adcea6d | 339 | buzzer = 1; wait(0.3); buzzer = 0; wait(0.3); |
neoqased | 0:d7629adcea6d | 340 | break; |
neoqased | 0:d7629adcea6d | 341 | case -1: //error * - - - - |
neoqased | 0:d7629adcea6d | 342 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 343 | buzzer = 1; wait(0.3); buzzer = 0; wait(0.3); |
neoqased | 0:d7629adcea6d | 344 | buzzer = 1; wait(0.3); buzzer = 0; wait(0.3); |
neoqased | 0:d7629adcea6d | 345 | buzzer = 1; wait(0.3); buzzer = 0; wait(0.3); |
neoqased | 0:d7629adcea6d | 346 | buzzer = 1; wait(0.3); buzzer = 0; wait(0.3); |
neoqased | 0:d7629adcea6d | 347 | break; |
neoqased | 0:d7629adcea6d | 348 | /**************エラーここまで**************/ |
neoqased | 0:d7629adcea6d | 349 | case 0: //サウンドなし |
neoqased | 0:d7629adcea6d | 350 | buzzer = 0; |
neoqased | 0:d7629adcea6d | 351 | break; |
neoqased | 0:d7629adcea6d | 352 | /**************状態を知らせるためのbeep**************/ |
neoqased | 0:d7629adcea6d | 353 | case 1: // *(短) |
neoqased | 0:d7629adcea6d | 354 | buzzer = 1; wait(0.1); buzzer = 0; |
neoqased | 0:d7629adcea6d | 355 | break; |
neoqased | 0:d7629adcea6d | 356 | case 2: // * * |
neoqased | 0:d7629adcea6d | 357 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.05); |
neoqased | 0:d7629adcea6d | 358 | buzzer = 1; wait(0.1); buzzer = 0; |
neoqased | 0:d7629adcea6d | 359 | break; |
neoqased | 0:d7629adcea6d | 360 | case 3: // -(長) |
neoqased | 0:d7629adcea6d | 361 | buzzer = 1; wait(0.3); buzzer = 0; |
neoqased | 0:d7629adcea6d | 362 | break; |
neoqased | 0:d7629adcea6d | 363 | case 4: // - - |
neoqased | 0:d7629adcea6d | 364 | buzzer = 1; wait(0.3); buzzer = 0; wait(0.3); |
neoqased | 0:d7629adcea6d | 365 | buzzer = 1; wait(0.3); buzzer = 0; |
neoqased | 0:d7629adcea6d | 366 | break; |
neoqased | 0:d7629adcea6d | 367 | case 5: // --- |
neoqased | 0:d7629adcea6d | 368 | buzzer = 1; wait(0.9); buzzer = 0; |
neoqased | 0:d7629adcea6d | 369 | break; |
neoqased | 0:d7629adcea6d | 370 | case 6: // * * * * * * * * * * |
neoqased | 0:d7629adcea6d | 371 | for(int i = 0; i < 3; i++){ |
neoqased | 0:d7629adcea6d | 372 | for(int j = 0; j < 3; j++){ |
neoqased | 0:d7629adcea6d | 373 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 374 | } |
neoqased | 0:d7629adcea6d | 375 | wait(0.2); |
neoqased | 0:d7629adcea6d | 376 | } |
neoqased | 0:d7629adcea6d | 377 | buzzer = 1; wait(0.1); buzzer = 0; |
neoqased | 0:d7629adcea6d | 378 | break; |
neoqased | 0:d7629adcea6d | 379 | case 7: // **-* ** -* ** *** **** "finish" |
neoqased | 0:d7629adcea6d | 380 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 381 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 382 | buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 383 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 384 | wait(0.2); |
neoqased | 0:d7629adcea6d | 385 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 386 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 387 | wait(0.2); |
neoqased | 0:d7629adcea6d | 388 | buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 389 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 390 | wait(0.2); |
neoqased | 0:d7629adcea6d | 391 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 392 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 393 | wait(0.2); |
neoqased | 0:d7629adcea6d | 394 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 395 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 396 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 397 | wait(0.2); |
neoqased | 0:d7629adcea6d | 398 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 399 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 400 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 401 | buzzer = 1; wait(0.1); buzzer = 0; |
neoqased | 0:d7629adcea6d | 402 | break; |
neoqased | 0:d7629adcea6d | 403 | case 8: // *-*** -*- --* "オワリ" |
neoqased | 0:d7629adcea6d | 404 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 405 | buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 406 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 407 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 408 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 409 | wait(0.2); |
neoqased | 0:d7629adcea6d | 410 | buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 411 | buzzer = 1; wait(0.1); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 412 | buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 413 | wait(0.2); |
neoqased | 0:d7629adcea6d | 414 | buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 415 | buzzer = 1; wait(0.3); buzzer = 0; wait(0.1); |
neoqased | 0:d7629adcea6d | 416 | buzzer = 1; wait(0.1); buzzer = 0; |
neoqased | 0:d7629adcea6d | 417 | break; |
neoqased | 0:d7629adcea6d | 418 | /**************状態を知らせるためのbeepここまで**************/ |
neoqased | 0:d7629adcea6d | 419 | default: //no sound |
neoqased | 0:d7629adcea6d | 420 | buzzer = 0; |
neoqased | 0:d7629adcea6d | 421 | break; |
neoqased | 0:d7629adcea6d | 422 | } |
neoqased | 0:d7629adcea6d | 423 | return 0; |
neoqased | 0:d7629adcea6d | 424 | } |
neoqased | 0:d7629adcea6d | 425 | |
neoqased | 0:d7629adcea6d | 426 | |
neoqased | 0:d7629adcea6d | 427 | int find_header(FILE *fp, char keyword[]) |
neoqased | 0:d7629adcea6d | 428 | /* |
neoqased | 0:d7629adcea6d | 429 | 入力 fp ファイルポインタ |
neoqased | 0:d7629adcea6d | 430 | keyword キーワード |
neoqased | 0:d7629adcea6d | 431 | 戻り値 1 : ヘッダを見つけた 0 ヘッダが見つからなかった |
neoqased | 0:d7629adcea6d | 432 | */ |
neoqased | 0:d7629adcea6d | 433 | { |
neoqased | 0:d7629adcea6d | 434 | char char_buf[256]; |
neoqased | 0:d7629adcea6d | 435 | char keyword_buf[256]; |
neoqased | 0:d7629adcea6d | 436 | char *condition; |
neoqased | 0:d7629adcea6d | 437 | int len,i; |
neoqased | 0:d7629adcea6d | 438 | |
neoqased | 0:d7629adcea6d | 439 | strcpy(keyword_buf,keyword); |
neoqased | 0:d7629adcea6d | 440 | rm_right_space(keyword_buf); /* 右側の空白を取り除く */ |
neoqased | 0:d7629adcea6d | 441 | |
neoqased | 0:d7629adcea6d | 442 | rewind(fp); |
neoqased | 0:d7629adcea6d | 443 | for(;;){ |
neoqased | 0:d7629adcea6d | 444 | condition = fgets(char_buf,BUFSIZ,fp); |
neoqased | 0:d7629adcea6d | 445 | if ( condition == NULL ) break; |
neoqased | 0:d7629adcea6d | 446 | rm_right_space(char_buf); /* 右側の空白と改行記号を取り除く */ |
neoqased | 0:d7629adcea6d | 447 | len = strlen(char_buf); |
neoqased | 0:d7629adcea6d | 448 | if ( len == 0 ) continue; /* この文はなくてもよい */ |
neoqased | 0:d7629adcea6d | 449 | for ( i = 0 ; i < len ; i++ ){ /* 左側の空白を取り除く */ |
neoqased | 0:d7629adcea6d | 450 | if ( char_buf[i] != ' ' ) break; |
neoqased | 0:d7629adcea6d | 451 | } |
neoqased | 0:d7629adcea6d | 452 | if ( strcmp(&char_buf[i],keyword_buf)==0){ |
neoqased | 0:d7629adcea6d | 453 | return(1); |
neoqased | 0:d7629adcea6d | 454 | break; |
neoqased | 0:d7629adcea6d | 455 | } |
neoqased | 0:d7629adcea6d | 456 | } |
neoqased | 0:d7629adcea6d | 457 | fprintf(stderr,"cannot find header. key word : |%s|\n",keyword); |
neoqased | 0:d7629adcea6d | 458 | Buzzer(-3); |
neoqased | 0:d7629adcea6d | 459 | return(0); |
neoqased | 0:d7629adcea6d | 460 | } |
neoqased | 0:d7629adcea6d | 461 | |
neoqased | 0:d7629adcea6d | 462 | /* 文字列の右側の空白を削除する 改行記号がある場合はそれも除去する */ |
neoqased | 0:d7629adcea6d | 463 | |
neoqased | 0:d7629adcea6d | 464 | void rm_right_space(char str[]) |
neoqased | 0:d7629adcea6d | 465 | { |
neoqased | 0:d7629adcea6d | 466 | int i; |
neoqased | 0:d7629adcea6d | 467 | |
neoqased | 0:d7629adcea6d | 468 | i = strlen(str)-1; |
neoqased | 0:d7629adcea6d | 469 | if ( str[i] == '\n' ) i--; |
neoqased | 0:d7629adcea6d | 470 | |
neoqased | 0:d7629adcea6d | 471 | while(i>=0){ |
neoqased | 0:d7629adcea6d | 472 | if ( str[i] != ' ') break; |
neoqased | 0:d7629adcea6d | 473 | i--; |
neoqased | 0:d7629adcea6d | 474 | } |
neoqased | 0:d7629adcea6d | 475 | str[i+1] = '\0'; |
neoqased | 0:d7629adcea6d | 476 | } |