量産型スピナーのプログラムです。 IRC Helicopter "SWIFT" のPropoでコントロールします。
Dependencies: CodecIRPropoSwift Motordriver Propo_RemotoIR mbed
main.cpp@0:9d3b35ce3c45, 2013-08-03 (annotated)
- Committer:
- suupen
- Date:
- Sat Aug 03 01:48:34 2013 +0000
- Revision:
- 0:9d3b35ce3c45
????????????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
suupen | 0:9d3b35ce3c45 | 1 | #include "mbed.h" |
suupen | 0:9d3b35ce3c45 | 2 | #include "math.h" |
suupen | 0:9d3b35ce3c45 | 3 | #include "motordriver.h" |
suupen | 0:9d3b35ce3c45 | 4 | #include "ReceiverIR.h" |
suupen | 0:9d3b35ce3c45 | 5 | |
suupen | 0:9d3b35ce3c45 | 6 | #include "CodecSwift.h" |
suupen | 0:9d3b35ce3c45 | 7 | #include "DecodeSwift.h" |
suupen | 0:9d3b35ce3c45 | 8 | |
suupen | 0:9d3b35ce3c45 | 9 | |
suupen | 0:9d3b35ce3c45 | 10 | Serial pc(USBTX,USBRX); |
suupen | 0:9d3b35ce3c45 | 11 | |
suupen | 0:9d3b35ce3c45 | 12 | ReceiverIR ir_rx(p30); |
suupen | 0:9d3b35ce3c45 | 13 | RemoteIR::Format format; |
suupen | 0:9d3b35ce3c45 | 14 | uint8_t buf[32]; |
suupen | 0:9d3b35ce3c45 | 15 | int bitcount; |
suupen | 0:9d3b35ce3c45 | 16 | |
suupen | 0:9d3b35ce3c45 | 17 | DecodeSwift swift; |
suupen | 0:9d3b35ce3c45 | 18 | CodecSwift::swiftPropo_t swiftData; |
suupen | 0:9d3b35ce3c45 | 19 | CodecSwift::normalizePropo_t normalize; |
suupen | 0:9d3b35ce3c45 | 20 | |
suupen | 0:9d3b35ce3c45 | 21 | |
suupen | 0:9d3b35ce3c45 | 22 | Motor Right(p24, p27, p26, 1); |
suupen | 0:9d3b35ce3c45 | 23 | Motor Left(p22, p14, p13, 1); |
suupen | 0:9d3b35ce3c45 | 24 | |
suupen | 0:9d3b35ce3c45 | 25 | Timeout timeout; |
suupen | 0:9d3b35ce3c45 | 26 | |
suupen | 0:9d3b35ce3c45 | 27 | void isr_timeout(void) |
suupen | 0:9d3b35ce3c45 | 28 | { |
suupen | 0:9d3b35ce3c45 | 29 | Right.speed(0); |
suupen | 0:9d3b35ce3c45 | 30 | Left.speed(0); |
suupen | 0:9d3b35ce3c45 | 31 | } |
suupen | 0:9d3b35ce3c45 | 32 | |
suupen | 0:9d3b35ce3c45 | 33 | DigitalOut led1(LED1); |
suupen | 0:9d3b35ce3c45 | 34 | DigitalOut led2(LED2); |
suupen | 0:9d3b35ce3c45 | 35 | DigitalOut led3(LED3); |
suupen | 0:9d3b35ce3c45 | 36 | DigitalOut led4(LED4); |
suupen | 0:9d3b35ce3c45 | 37 | |
suupen | 0:9d3b35ce3c45 | 38 | int main() |
suupen | 0:9d3b35ce3c45 | 39 | { |
suupen | 0:9d3b35ce3c45 | 40 | pc.baud(38400); |
suupen | 0:9d3b35ce3c45 | 41 | |
suupen | 0:9d3b35ce3c45 | 42 | timeout.detach(); |
suupen | 0:9d3b35ce3c45 | 43 | timeout.attach_us(&isr_timeout, 100 * 1000); |
suupen | 0:9d3b35ce3c45 | 44 | |
suupen | 0:9d3b35ce3c45 | 45 | uint8_t count = 0; |
suupen | 0:9d3b35ce3c45 | 46 | float theta; |
suupen | 0:9d3b35ce3c45 | 47 | float x; |
suupen | 0:9d3b35ce3c45 | 48 | float y; |
suupen | 0:9d3b35ce3c45 | 49 | float r; |
suupen | 0:9d3b35ce3c45 | 50 | float l; |
suupen | 0:9d3b35ce3c45 | 51 | |
suupen | 0:9d3b35ce3c45 | 52 | |
suupen | 0:9d3b35ce3c45 | 53 | while(1) { |
suupen | 0:9d3b35ce3c45 | 54 | |
suupen | 0:9d3b35ce3c45 | 55 | // IR data Monitor |
suupen | 0:9d3b35ce3c45 | 56 | led1 = 0; |
suupen | 0:9d3b35ce3c45 | 57 | led2 = 0; |
suupen | 0:9d3b35ce3c45 | 58 | led3 = 0; |
suupen | 0:9d3b35ce3c45 | 59 | led4 = 0; |
suupen | 0:9d3b35ce3c45 | 60 | |
suupen | 0:9d3b35ce3c45 | 61 | switch(ir_rx.getState()) { |
suupen | 0:9d3b35ce3c45 | 62 | case ReceiverIR::Idle: |
suupen | 0:9d3b35ce3c45 | 63 | led1 = 1; |
suupen | 0:9d3b35ce3c45 | 64 | break; |
suupen | 0:9d3b35ce3c45 | 65 | case ReceiverIR::Receiving: |
suupen | 0:9d3b35ce3c45 | 66 | led2 = 1; |
suupen | 0:9d3b35ce3c45 | 67 | break; |
suupen | 0:9d3b35ce3c45 | 68 | case ReceiverIR::Received: |
suupen | 0:9d3b35ce3c45 | 69 | led3 = 1; |
suupen | 0:9d3b35ce3c45 | 70 | break; |
suupen | 0:9d3b35ce3c45 | 71 | default: |
suupen | 0:9d3b35ce3c45 | 72 | led4 = 1; |
suupen | 0:9d3b35ce3c45 | 73 | break; |
suupen | 0:9d3b35ce3c45 | 74 | } |
suupen | 0:9d3b35ce3c45 | 75 | |
suupen | 0:9d3b35ce3c45 | 76 | |
suupen | 0:9d3b35ce3c45 | 77 | // IR data to Motor data |
suupen | 0:9d3b35ce3c45 | 78 | if (ir_rx.getState() == ReceiverIR::Received) { |
suupen | 0:9d3b35ce3c45 | 79 | bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8); |
suupen | 0:9d3b35ce3c45 | 80 | |
suupen | 0:9d3b35ce3c45 | 81 | bool ans = swift.normalize(buf, &normalize); |
suupen | 0:9d3b35ce3c45 | 82 | |
suupen | 0:9d3b35ce3c45 | 83 | |
suupen | 0:9d3b35ce3c45 | 84 | if(ans == true) { |
suupen | 0:9d3b35ce3c45 | 85 | timeout.detach(); |
suupen | 0:9d3b35ce3c45 | 86 | timeout.attach_us(&isr_timeout, 100 * 1000); |
suupen | 0:9d3b35ce3c45 | 87 | |
suupen | 0:9d3b35ce3c45 | 88 | theta = atan2f(normalize.elevator, normalize.ladder); |
suupen | 0:9d3b35ce3c45 | 89 | x = -normalize.slottle * cosf(theta); |
suupen | 0:9d3b35ce3c45 | 90 | y = -normalize.slottle * sinf(theta); |
suupen | 0:9d3b35ce3c45 | 91 | |
suupen | 0:9d3b35ce3c45 | 92 | if((fabsf(normalize.elevator) < 0.01) && (fabsf(normalize.ladder) < 0.01)) { |
suupen | 0:9d3b35ce3c45 | 93 | r = 0; |
suupen | 0:9d3b35ce3c45 | 94 | l = 0; |
suupen | 0:9d3b35ce3c45 | 95 | } else if(x > 0.0) { |
suupen | 0:9d3b35ce3c45 | 96 | r = (y - x / 2.0); |
suupen | 0:9d3b35ce3c45 | 97 | l = (y + x / 2.0); |
suupen | 0:9d3b35ce3c45 | 98 | } else { |
suupen | 0:9d3b35ce3c45 | 99 | r = (y - x / 2.0); |
suupen | 0:9d3b35ce3c45 | 100 | l = (y + x / 2.0); |
suupen | 0:9d3b35ce3c45 | 101 | } |
suupen | 0:9d3b35ce3c45 | 102 | |
suupen | 0:9d3b35ce3c45 | 103 | #if 0 |
suupen | 0:9d3b35ce3c45 | 104 | printf("x = %f y = %f r = %f l = %f\n",x, y, r, l); |
suupen | 0:9d3b35ce3c45 | 105 | #endif |
suupen | 0:9d3b35ce3c45 | 106 | |
suupen | 0:9d3b35ce3c45 | 107 | Right.speed(r); |
suupen | 0:9d3b35ce3c45 | 108 | Left.speed(-l); |
suupen | 0:9d3b35ce3c45 | 109 | } else if( ++count > 10) { |
suupen | 0:9d3b35ce3c45 | 110 | count = 0; |
suupen | 0:9d3b35ce3c45 | 111 | Right.speed(0); |
suupen | 0:9d3b35ce3c45 | 112 | Left.speed(0); |
suupen | 0:9d3b35ce3c45 | 113 | } |
suupen | 0:9d3b35ce3c45 | 114 | |
suupen | 0:9d3b35ce3c45 | 115 | #if 0 |
suupen | 0:9d3b35ce3c45 | 116 | if(ans == true) { |
suupen | 0:9d3b35ce3c45 | 117 | printf("count = %02x band = %1d slottle = %f trim = %f ladder = %f elevator = %f\n",normalize.count,normalize.band,normalize.slottle,normalize.trim,normalize.ladder, normalize.elevator); |
suupen | 0:9d3b35ce3c45 | 118 | } else { |
suupen | 0:9d3b35ce3c45 | 119 | printf("NG\n"); |
suupen | 0:9d3b35ce3c45 | 120 | } |
suupen | 0:9d3b35ce3c45 | 121 | #endif |
suupen | 0:9d3b35ce3c45 | 122 | } |
suupen | 0:9d3b35ce3c45 | 123 | |
suupen | 0:9d3b35ce3c45 | 124 | |
suupen | 0:9d3b35ce3c45 | 125 | |
suupen | 0:9d3b35ce3c45 | 126 | } |
suupen | 0:9d3b35ce3c45 | 127 | } |