suu pen / Mbed 2 deprecated spinner2

量産型スピナーのプログラムです。 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?

UserRevisionLine numberNew 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 }