realtime process control with RTOS
Fork of mbed-os-example-mbed5-blinky by
main.cpp@23:bc5ad77d56bb, 2018-10-07 (annotated)
- Committer:
- ykuroda
- Date:
- Sun Oct 07 12:51:03 2018 +0000
- Revision:
- 23:bc5ad77d56bb
- Parent:
- 22:f0164a2a09b0
- Child:
- 24:e414c7234bcc
Mechatro code v1
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ykuroda | 20:13fcc82f4cab | 1 | // mbed-os-io-control |
ykuroda | 20:13fcc82f4cab | 2 | // I/O制御プログラム(機械工学実験1) |
ykuroda | 19:9381d775bfc5 | 3 | // |
ykuroda | 20:13fcc82f4cab | 4 | // 20161027 ... v1.0 ... originally written by Y.Kuroda for Mechatronics |
ykuroda | 20:13fcc82f4cab | 5 | // 20180917 ... v2.0 ... customised for Jikken1(mbed LPC1768専用) |
ykuroda | 22:f0164a2a09b0 | 6 | // 20181004 ... v3.0 ... uses EventQueue |
ykuroda | 19:9381d775bfc5 | 7 | // |
Jonathan Austin |
0:2757d7abb7d9 | 8 | #include "mbed.h" |
ykuroda | 20:13fcc82f4cab | 9 | |
ykuroda | 23:bc5ad77d56bb | 10 | const int DELTA_T = 1; // mainスレッドの時間間隔の設定[ms](デフォルトは1ms) |
ykuroda | 20:13fcc82f4cab | 11 | int delta_t = DELTA_T; |
ykuroda | 20:13fcc82f4cab | 12 | int qbits = 6; // 量子化シフト値 |
ykuroda | 20:13fcc82f4cab | 13 | DigitalOut led1(LED1); |
ykuroda | 20:13fcc82f4cab | 14 | DigitalOut led2(LED2); |
ykuroda | 23:bc5ad77d56bb | 15 | //PwmOut led3(LED3); |
ykuroda | 20:13fcc82f4cab | 16 | DigitalOut led4(LED4); |
ykuroda | 20:13fcc82f4cab | 17 | |
ykuroda | 20:13fcc82f4cab | 18 | // p11,12 サンプリング周期設定ピン(デフォルトは0) |
ykuroda | 22:f0164a2a09b0 | 19 | // p11上位ビット,p12下位ビット.0:1kHz, 1: 100Hz, 2: 10Hz, 3: 1Hz |
ykuroda | 20:13fcc82f4cab | 20 | InterruptIn samplingtime_MSB(p11); |
ykuroda | 20:13fcc82f4cab | 21 | InterruptIn samplingtime_LSB(p12); |
Jonathan Austin |
0:2757d7abb7d9 | 22 | |
ykuroda | 20:13fcc82f4cab | 23 | // p13,14 量子化粒度設定ピン(デフォルトは0) |
ykuroda | 20:13fcc82f4cab | 24 | // p13上位ビット,p14下位ビット.0:10bit, 1:8bit, 2:6bit, 3:4bit |
ykuroda | 20:13fcc82f4cab | 25 | InterruptIn quantization_MSB(p13); |
ykuroda | 20:13fcc82f4cab | 26 | InterruptIn quantization_LSB(p14); |
ykuroda | 20:13fcc82f4cab | 27 | |
ykuroda | 20:13fcc82f4cab | 28 | // アナログ入出力ピン(入出力共に0 - 3.3V) |
ykuroda | 20:13fcc82f4cab | 29 | AnalogOut aout(p18); |
ykuroda | 20:13fcc82f4cab | 30 | AnalogIn ain(p20); |
Jonathan Austin |
0:2757d7abb7d9 | 31 | |
ykuroda | 20:13fcc82f4cab | 32 | // デジタル入力ピン(割り込みにより実現) |
ykuroda | 20:13fcc82f4cab | 33 | InterruptIn din1(p21); |
ykuroda | 20:13fcc82f4cab | 34 | InterruptIn din2(p22); |
ykuroda | 20:13fcc82f4cab | 35 | |
ykuroda | 20:13fcc82f4cab | 36 | // 方形波,パルス波出力ピン |
ykuroda | 20:13fcc82f4cab | 37 | PwmOut squarewave(p23); |
ykuroda | 20:13fcc82f4cab | 38 | PwmOut pulsewave(p24); |
ykuroda | 20:13fcc82f4cab | 39 | |
ykuroda | 23:bc5ad77d56bb | 40 | const int SIG_ST = 0x1; // signal番号 |
ykuroda | 23:bc5ad77d56bb | 41 | Thread* sthread; |
ykuroda | 23:bc5ad77d56bb | 42 | EventQueue queue; |
ykuroda | 23:bc5ad77d56bb | 43 | int id = 0; |
ykuroda | 23:bc5ad77d56bb | 44 | |
ykuroda | 20:13fcc82f4cab | 45 | // サンプリング処理をするスレッド(時間管理された処理) |
ykuroda | 23:bc5ad77d56bb | 46 | void sampling_job(void* v) |
ykuroda | 23:bc5ad77d56bb | 47 | { |
ykuroda | 22:f0164a2a09b0 | 48 | unsigned short a_data = ain.read_u16(); // AD入力(ADは12ビット) |
ykuroda | 22:f0164a2a09b0 | 49 | aout.write_u16((a_data>>qbits)<<qbits);// DA出力(量子化粒度はピンにより設定) |
ykuroda | 23:bc5ad77d56bb | 50 | led4 = !led4; |
ykuroda | 23:bc5ad77d56bb | 51 | } |
ykuroda | 23:bc5ad77d56bb | 52 | |
ykuroda | 23:bc5ad77d56bb | 53 | void setup_thread(void const* arg) // 設定処理をするスレッド |
ykuroda | 23:bc5ad77d56bb | 54 | { |
ykuroda | 23:bc5ad77d56bb | 55 | while(true) { |
ykuroda | 23:bc5ad77d56bb | 56 | Thread::signal_wait(SIG_ST); // シグナルを待つ |
ykuroda | 23:bc5ad77d56bb | 57 | |
ykuroda | 23:bc5ad77d56bb | 58 | // ピンの状態を読み取り,2ビット値に変換 |
ykuroda | 23:bc5ad77d56bb | 59 | int s_msb = 0, s_lsb = 0; |
ykuroda | 23:bc5ad77d56bb | 60 | int q_msb = 0, q_lsb = 0; |
ykuroda | 23:bc5ad77d56bb | 61 | const int RETRY = 1000; |
ykuroda | 23:bc5ad77d56bb | 62 | for(int i=0; i<RETRY; i++) { |
ykuroda | 23:bc5ad77d56bb | 63 | s_msb += samplingtime_MSB; |
ykuroda | 23:bc5ad77d56bb | 64 | s_lsb += samplingtime_LSB; |
ykuroda | 23:bc5ad77d56bb | 65 | q_msb += quantization_MSB; |
ykuroda | 23:bc5ad77d56bb | 66 | q_lsb += quantization_LSB; |
ykuroda | 23:bc5ad77d56bb | 67 | } |
ykuroda | 23:bc5ad77d56bb | 68 | s_msb /= RETRY; s_lsb /= RETRY; |
ykuroda | 23:bc5ad77d56bb | 69 | q_msb /= RETRY; q_lsb /= RETRY; |
ykuroda | 23:bc5ad77d56bb | 70 | |
ykuroda | 23:bc5ad77d56bb | 71 | int samplingtime = (s_msb<<1)&0x2 | s_lsb&0x1; |
ykuroda | 23:bc5ad77d56bb | 72 | |
ykuroda | 23:bc5ad77d56bb | 73 | // 制御周期を10^nとする(n=0:1s, n=1:0.1s, n=2:0.01s, n=3:0.001s) |
ykuroda | 23:bc5ad77d56bb | 74 | delta_t = (int)pow(10.0,(double)samplingtime); |
ykuroda | 23:bc5ad77d56bb | 75 | |
ykuroda | 23:bc5ad77d56bb | 76 | |
ykuroda | 23:bc5ad77d56bb | 77 | squarewave.period_ms(delta_t); // 方形波の周期の設定 |
ykuroda | 23:bc5ad77d56bb | 78 | pulsewave.period_ms(delta_t); // パルス波の周期の設定 |
ykuroda | 23:bc5ad77d56bb | 79 | squarewave.write(0.5F); // duty比 |
ykuroda | 23:bc5ad77d56bb | 80 | pulsewave.write(0.1F); // duty比 |
ykuroda | 23:bc5ad77d56bb | 81 | |
ykuroda | 23:bc5ad77d56bb | 82 | // 量子化設定ピン(p13,14)の設定を読んで量子化粒度を決定する |
ykuroda | 23:bc5ad77d56bb | 83 | // 設定ピンの状態を読み取り,2ビット値に変換 |
ykuroda | 23:bc5ad77d56bb | 84 | int quantization = (q_msb<<1)&0x2 | q_lsb&0x1; |
ykuroda | 23:bc5ad77d56bb | 85 | qbits = 6+quantization*4; // 量子化サイズの決定 |
ykuroda | 23:bc5ad77d56bb | 86 | // (デジタル出力時の量子化サイズは 0:1024, 1:256, 2:64, 3:16 steps) |
ykuroda | 23:bc5ad77d56bb | 87 | |
ykuroda | 23:bc5ad77d56bb | 88 | |
ykuroda | 23:bc5ad77d56bb | 89 | queue.cancel(id); |
ykuroda | 23:bc5ad77d56bb | 90 | printf("delta_t = %d, qbits = %d\n", delta_t, qbits); |
ykuroda | 23:bc5ad77d56bb | 91 | id = queue.call_every(delta_t, sampling_job, (void*)0); |
ykuroda | 23:bc5ad77d56bb | 92 | } |
Jonathan Austin |
0:2757d7abb7d9 | 93 | } |
Jonathan Austin |
1:846c97078558 | 94 | |
ykuroda | 20:13fcc82f4cab | 95 | // 量子化粒度設定ハンドラ |
ykuroda | 23:bc5ad77d56bb | 96 | void qsize_handler(void) |
ykuroda | 23:bc5ad77d56bb | 97 | { |
ykuroda | 23:bc5ad77d56bb | 98 | sthread->signal_set(SIG_ST); // スレッドへ再開シグナルを送る |
ykuroda | 22:f0164a2a09b0 | 99 | } |
ykuroda | 20:13fcc82f4cab | 100 | |
ykuroda | 20:13fcc82f4cab | 101 | // サンプリングタイム設定割り込みハンドラ...時間設定ピンの状態が変化した時に呼び出される |
ykuroda | 23:bc5ad77d56bb | 102 | void pinstate_handler(void) |
ykuroda | 23:bc5ad77d56bb | 103 | { |
ykuroda | 23:bc5ad77d56bb | 104 | sthread->signal_set(SIG_ST); // スレッドへ再開シグナルを送る |
ykuroda | 20:13fcc82f4cab | 105 | } |
ykuroda | 20:13fcc82f4cab | 106 | |
ykuroda | 20:13fcc82f4cab | 107 | // デジタル入力割り込みハンドラ...デジタル入力信号の状態が変化した時に呼び出される |
ykuroda | 23:bc5ad77d56bb | 108 | void din_handler(void) |
ykuroda | 23:bc5ad77d56bb | 109 | { |
ykuroda | 20:13fcc82f4cab | 110 | led1 = din1; // ピンの状態をそのままLEDの点灯状態にする |
ykuroda | 20:13fcc82f4cab | 111 | led2 = din2; |
ykuroda | 21:967504024346 | 112 | } |
ykuroda | 21:967504024346 | 113 | |
ykuroda | 23:bc5ad77d56bb | 114 | int main() |
ykuroda | 23:bc5ad77d56bb | 115 | { |
ykuroda | 23:bc5ad77d56bb | 116 | sthread = new Thread(setup_thread); // 設定用スレッドを起動 |
ykuroda | 23:bc5ad77d56bb | 117 | |
ykuroda | 23:bc5ad77d56bb | 118 | |
ykuroda | 20:13fcc82f4cab | 119 | squarewave.period_ms(delta_t); // 初期周期の設定 |
ykuroda | 20:13fcc82f4cab | 120 | pulsewave.period_ms(delta_t); // 初期周期の設定 |
ykuroda | 20:13fcc82f4cab | 121 | squarewave.write(0.5F); // 初期duty比(方形波=50%) |
ykuroda | 20:13fcc82f4cab | 122 | pulsewave.write(0.1F); // 初期duty比(パルス波=10%) |
ykuroda | 20:13fcc82f4cab | 123 | |
ykuroda | 22:f0164a2a09b0 | 124 | samplingtime_MSB.disable_irq(); |
ykuroda | 22:f0164a2a09b0 | 125 | samplingtime_MSB.mode(PullDown); |
ykuroda | 20:13fcc82f4cab | 126 | samplingtime_MSB.rise(pinstate_handler);// 周期設定ハンドラの設定. |
ykuroda | 20:13fcc82f4cab | 127 | samplingtime_MSB.fall(pinstate_handler);// 設定ピンの状態変化で |
ykuroda | 22:f0164a2a09b0 | 128 | samplingtime_MSB.enable_irq(); |
ykuroda | 23:bc5ad77d56bb | 129 | |
ykuroda | 22:f0164a2a09b0 | 130 | samplingtime_LSB.disable_irq(); |
ykuroda | 22:f0164a2a09b0 | 131 | samplingtime_LSB.mode(PullDown); |
ykuroda | 20:13fcc82f4cab | 132 | samplingtime_LSB.rise(pinstate_handler);// ハンドラが呼び出される |
ykuroda | 20:13fcc82f4cab | 133 | samplingtime_LSB.fall(pinstate_handler);// ようにする |
ykuroda | 22:f0164a2a09b0 | 134 | samplingtime_LSB.enable_irq(); |
ykuroda | 23:bc5ad77d56bb | 135 | |
ykuroda | 23:bc5ad77d56bb | 136 | quantization_MSB.disable_irq(); |
ykuroda | 23:bc5ad77d56bb | 137 | quantization_MSB.mode(PullDown); |
ykuroda | 20:13fcc82f4cab | 138 | quantization_MSB.rise(qsize_handler); // 量子化粒度設定ハンドラの設定 |
ykuroda | 20:13fcc82f4cab | 139 | quantization_MSB.fall(qsize_handler); |
ykuroda | 23:bc5ad77d56bb | 140 | quantization_MSB.enable_irq(); |
ykuroda | 23:bc5ad77d56bb | 141 | |
ykuroda | 23:bc5ad77d56bb | 142 | quantization_LSB.disable_irq(); |
ykuroda | 23:bc5ad77d56bb | 143 | quantization_LSB.mode(PullDown); |
ykuroda | 20:13fcc82f4cab | 144 | quantization_LSB.rise(qsize_handler); |
ykuroda | 20:13fcc82f4cab | 145 | quantization_LSB.fall(qsize_handler); |
ykuroda | 23:bc5ad77d56bb | 146 | quantization_LSB.enable_irq(); |
ykuroda | 23:bc5ad77d56bb | 147 | |
ykuroda | 23:bc5ad77d56bb | 148 | din1.disable_irq(); |
ykuroda | 23:bc5ad77d56bb | 149 | din1.mode(PullDown); |
ykuroda | 20:13fcc82f4cab | 150 | din1.rise(&din_handler); // デジタル入力ハンドラの設定 |
ykuroda | 20:13fcc82f4cab | 151 | din1.fall(&din_handler); // din1,2の状態変化でハンドラ |
ykuroda | 23:bc5ad77d56bb | 152 | din1.enable_irq(); |
ykuroda | 23:bc5ad77d56bb | 153 | |
ykuroda | 23:bc5ad77d56bb | 154 | din2.disable_irq(); |
ykuroda | 23:bc5ad77d56bb | 155 | din2.mode(PullDown); |
ykuroda | 20:13fcc82f4cab | 156 | din2.rise(&din_handler); // が呼ばれる |
ykuroda | 20:13fcc82f4cab | 157 | din2.fall(&din_handler); |
ykuroda | 23:bc5ad77d56bb | 158 | din2.enable_irq(); |
ykuroda | 20:13fcc82f4cab | 159 | |
ykuroda | 23:bc5ad77d56bb | 160 | id = queue.call_every(delta_t, sampling_job, (void*)0); |
ykuroda | 22:f0164a2a09b0 | 161 | queue.dispatch(); |
ykuroda | 20:13fcc82f4cab | 162 | } |