量産型スピナーのプログラムです。 IRC Helicopter "SWIFT" のPropoでコントロールします。
Dependencies: CodecIRPropoSwift Motordriver Propo_RemotoIR mbed
main.cpp
- Committer:
- suupen
- Date:
- 2013-08-03
- Revision:
- 0:9d3b35ce3c45
File content as of revision 0:9d3b35ce3c45:
#include "mbed.h" #include "math.h" #include "motordriver.h" #include "ReceiverIR.h" #include "CodecSwift.h" #include "DecodeSwift.h" Serial pc(USBTX,USBRX); ReceiverIR ir_rx(p30); RemoteIR::Format format; uint8_t buf[32]; int bitcount; DecodeSwift swift; CodecSwift::swiftPropo_t swiftData; CodecSwift::normalizePropo_t normalize; Motor Right(p24, p27, p26, 1); Motor Left(p22, p14, p13, 1); Timeout timeout; void isr_timeout(void) { Right.speed(0); Left.speed(0); } DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); int main() { pc.baud(38400); timeout.detach(); timeout.attach_us(&isr_timeout, 100 * 1000); uint8_t count = 0; float theta; float x; float y; float r; float l; while(1) { // IR data Monitor led1 = 0; led2 = 0; led3 = 0; led4 = 0; switch(ir_rx.getState()) { case ReceiverIR::Idle: led1 = 1; break; case ReceiverIR::Receiving: led2 = 1; break; case ReceiverIR::Received: led3 = 1; break; default: led4 = 1; break; } // IR data to Motor data if (ir_rx.getState() == ReceiverIR::Received) { bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8); bool ans = swift.normalize(buf, &normalize); if(ans == true) { timeout.detach(); timeout.attach_us(&isr_timeout, 100 * 1000); theta = atan2f(normalize.elevator, normalize.ladder); x = -normalize.slottle * cosf(theta); y = -normalize.slottle * sinf(theta); if((fabsf(normalize.elevator) < 0.01) && (fabsf(normalize.ladder) < 0.01)) { r = 0; l = 0; } else if(x > 0.0) { r = (y - x / 2.0); l = (y + x / 2.0); } else { r = (y - x / 2.0); l = (y + x / 2.0); } #if 0 printf("x = %f y = %f r = %f l = %f\n",x, y, r, l); #endif Right.speed(r); Left.speed(-l); } else if( ++count > 10) { count = 0; Right.speed(0); Left.speed(0); } #if 0 if(ans == true) { 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); } else { printf("NG\n"); } #endif } } }