お盆用コードだよ
Dependencies: Servo mbed-rtos mbed
main.cpp@2:0edba6ddfd48, 2014-06-28 (annotated)
- Committer:
- Hatter
- Date:
- Sat Jun 28 07:08:59 2014 +0000
- Revision:
- 2:0edba6ddfd48
- Parent:
- 1:17b3afbf9278
RTOS??????????????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Hatter | 0:72dbc49bd254 | 1 | /* |
Hatter | 0:72dbc49bd254 | 2 | * Servo motor program for OBON by Tomoki Hokida |
Hatter | 0:72dbc49bd254 | 3 | * |
Hatter | 2:0edba6ddfd48 | 4 | * Use RTOS in this program |
Hatter | 0:72dbc49bd254 | 5 | * |
Hatter | 0:72dbc49bd254 | 6 | * RTOSをバリバリ使用しているのであしあらず |
Hatter | 0:72dbc49bd254 | 7 | * サーボを4つ使うことになったらSERVO_4をdefineしましょう |
Hatter | 0:72dbc49bd254 | 8 | */ |
Hatter | 0:72dbc49bd254 | 9 | |
Hatter | 0:72dbc49bd254 | 10 | #include "mbed.h" |
Hatter | 0:72dbc49bd254 | 11 | #include "Servo.h" |
Hatter | 0:72dbc49bd254 | 12 | #include "rtos.h" |
Hatter | 0:72dbc49bd254 | 13 | |
Hatter | 0:72dbc49bd254 | 14 | #define DEFAULT_R 50 |
Hatter | 0:72dbc49bd254 | 15 | #define DEFAULT_L 50 |
Hatter | 0:72dbc49bd254 | 16 | #define DEFAULT_U 50 |
Hatter | 0:72dbc49bd254 | 17 | #define DEFAULT_D 50 |
Hatter | 0:72dbc49bd254 | 18 | |
Hatter | 2:0edba6ddfd48 | 19 | #define SERVO_4 |
Hatter | 1:17b3afbf9278 | 20 | |
Hatter | 0:72dbc49bd254 | 21 | DigitalOut led1(LED1); |
Hatter | 0:72dbc49bd254 | 22 | DigitalOut led2(LED2); |
Hatter | 0:72dbc49bd254 | 23 | |
Hatter | 0:72dbc49bd254 | 24 | #ifdef SERVO_4 |
Hatter | 0:72dbc49bd254 | 25 | DigitalOut led3(LED3); |
Hatter | 0:72dbc49bd254 | 26 | DigitalOut led4(LED4); |
Hatter | 0:72dbc49bd254 | 27 | #endif |
Hatter | 0:72dbc49bd254 | 28 | |
Hatter | 2:0edba6ddfd48 | 29 | DigitalIn sw[4] = {p17, p19, p18, p20}; |
Hatter | 0:72dbc49bd254 | 30 | |
Hatter | 2:0edba6ddfd48 | 31 | //DigitalIn swU(p17); |
Hatter | 2:0edba6ddfd48 | 32 | //DigitalIn swD(p18); |
Hatter | 2:0edba6ddfd48 | 33 | //DigitalIn swR(p19); |
Hatter | 2:0edba6ddfd48 | 34 | //DigitalIn swL(p20); |
Hatter | 2:0edba6ddfd48 | 35 | //Servo obonU(p21); |
Hatter | 2:0edba6ddfd48 | 36 | //Servo obonR(p22); |
Hatter | 2:0edba6ddfd48 | 37 | //Servo obonD(p23); |
Hatter | 2:0edba6ddfd48 | 38 | //Servo obonL(p24); |
Hatter | 2:0edba6ddfd48 | 39 | |
Hatter | 0:72dbc49bd254 | 40 | |
Hatter | 0:72dbc49bd254 | 41 | #ifdef SERVO_4 |
Hatter | 2:0edba6ddfd48 | 42 | |
Hatter | 2:0edba6ddfd48 | 43 | Servo obon[4] = { p21, p22, p23, p24}; |
Hatter | 2:0edba6ddfd48 | 44 | |
Hatter | 2:0edba6ddfd48 | 45 | #else |
Hatter | 2:0edba6ddfd48 | 46 | |
Hatter | 2:0edba6ddfd48 | 47 | Servo obon[2] = {p21,p22}; |
Hatter | 2:0edba6ddfd48 | 48 | |
Hatter | 0:72dbc49bd254 | 49 | #endif |
Hatter | 0:72dbc49bd254 | 50 | |
Hatter | 0:72dbc49bd254 | 51 | void obonThreadR(void const *argument){ |
Hatter | 0:72dbc49bd254 | 52 | |
Hatter | 0:72dbc49bd254 | 53 | for(int j=DEFAULT_R;;){ |
Hatter | 0:72dbc49bd254 | 54 | |
Hatter | 2:0edba6ddfd48 | 55 | obon[1] = j/100.0; |
Hatter | 0:72dbc49bd254 | 56 | |
Hatter | 2:0edba6ddfd48 | 57 | if((!sw[1])&&(j<100.0)){j++;} |
Hatter | 2:0edba6ddfd48 | 58 | if((!sw[3])&&(j>0)){j--;} |
Hatter | 0:72dbc49bd254 | 59 | |
Hatter | 0:72dbc49bd254 | 60 | led1 = !led1; |
Hatter | 0:72dbc49bd254 | 61 | |
Hatter | 0:72dbc49bd254 | 62 | Thread::wait(10); |
Hatter | 0:72dbc49bd254 | 63 | } |
Hatter | 0:72dbc49bd254 | 64 | } |
Hatter | 0:72dbc49bd254 | 65 | |
Hatter | 0:72dbc49bd254 | 66 | void obonThreadU(void const *argument){ |
Hatter | 0:72dbc49bd254 | 67 | for(int k=DEFAULT_U; ;){ |
Hatter | 0:72dbc49bd254 | 68 | |
Hatter | 2:0edba6ddfd48 | 69 | obon[0] = k/100.0; |
Hatter | 0:72dbc49bd254 | 70 | |
Hatter | 2:0edba6ddfd48 | 71 | if((!sw[0])&&(k<100.0)){k++;} |
Hatter | 2:0edba6ddfd48 | 72 | if((!sw[2])&&(k>0)){k--;} |
Hatter | 0:72dbc49bd254 | 73 | |
Hatter | 0:72dbc49bd254 | 74 | led2 = !led2; |
Hatter | 0:72dbc49bd254 | 75 | |
Hatter | 0:72dbc49bd254 | 76 | Thread::wait(10); |
Hatter | 0:72dbc49bd254 | 77 | } |
Hatter | 0:72dbc49bd254 | 78 | } |
Hatter | 0:72dbc49bd254 | 79 | |
Hatter | 0:72dbc49bd254 | 80 | #ifdef SERVO_4 |
Hatter | 0:72dbc49bd254 | 81 | |
Hatter | 0:72dbc49bd254 | 82 | void obonThreadL(void const *argument){ |
Hatter | 0:72dbc49bd254 | 83 | for(int i=DEFAULT_L;;){ |
Hatter | 0:72dbc49bd254 | 84 | |
Hatter | 2:0edba6ddfd48 | 85 | obon[3] = i/100.0; |
Hatter | 0:72dbc49bd254 | 86 | |
Hatter | 2:0edba6ddfd48 | 87 | if((!sw[3])&&(i<100.0)){i++;} |
Hatter | 2:0edba6ddfd48 | 88 | if((!sw[1])&&(i>0)){i--;} |
Hatter | 0:72dbc49bd254 | 89 | |
Hatter | 0:72dbc49bd254 | 90 | led3 = !led3; |
Hatter | 0:72dbc49bd254 | 91 | |
Hatter | 0:72dbc49bd254 | 92 | Thread::wait(10); |
Hatter | 0:72dbc49bd254 | 93 | } |
Hatter | 0:72dbc49bd254 | 94 | } |
Hatter | 0:72dbc49bd254 | 95 | |
Hatter | 0:72dbc49bd254 | 96 | void obonThreadD(void const *argument){ |
Hatter | 0:72dbc49bd254 | 97 | for(int l=DEFAULT_D;;){ |
Hatter | 0:72dbc49bd254 | 98 | |
Hatter | 2:0edba6ddfd48 | 99 | obon[2] = l/100.0; |
Hatter | 0:72dbc49bd254 | 100 | |
Hatter | 2:0edba6ddfd48 | 101 | if((!sw[2])&&(l<=100.0)){l++;} |
Hatter | 2:0edba6ddfd48 | 102 | if((!sw[1])&&(l>=0)){l--;} |
Hatter | 0:72dbc49bd254 | 103 | |
Hatter | 0:72dbc49bd254 | 104 | led4 = !led4; |
Hatter | 0:72dbc49bd254 | 105 | |
Hatter | 0:72dbc49bd254 | 106 | Thread::wait(10); |
Hatter | 0:72dbc49bd254 | 107 | } |
Hatter | 0:72dbc49bd254 | 108 | } |
Hatter | 0:72dbc49bd254 | 109 | #endif |
Hatter | 0:72dbc49bd254 | 110 | |
Hatter | 0:72dbc49bd254 | 111 | int main() { |
Hatter | 0:72dbc49bd254 | 112 | |
Hatter | 2:0edba6ddfd48 | 113 | sw[0].mode(PullUp); |
Hatter | 2:0edba6ddfd48 | 114 | sw[1].mode(PullUp); |
Hatter | 2:0edba6ddfd48 | 115 | sw[2].mode(PullUp); |
Hatter | 2:0edba6ddfd48 | 116 | sw[3].mode(PullUp); |
Hatter | 2:0edba6ddfd48 | 117 | |
Hatter | 0:72dbc49bd254 | 118 | |
Hatter | 0:72dbc49bd254 | 119 | Thread thread1(obonThreadU); |
Hatter | 0:72dbc49bd254 | 120 | Thread thread2(obonThreadR); |
Hatter | 0:72dbc49bd254 | 121 | |
Hatter | 2:0edba6ddfd48 | 122 | #ifdef SERVO_4 |
Hatter | 0:72dbc49bd254 | 123 | |
Hatter | 0:72dbc49bd254 | 124 | Thread thread3(obonThreadD); |
Hatter | 0:72dbc49bd254 | 125 | Thread thread4(obonThreadL); |
Hatter | 2:0edba6ddfd48 | 126 | |
Hatter | 2:0edba6ddfd48 | 127 | #endif |
Hatter | 0:72dbc49bd254 | 128 | for(;;); |
Hatter | 0:72dbc49bd254 | 129 | |
Hatter | 0:72dbc49bd254 | 130 | } |