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
        }



    }
}