File content as of revision 14:2fafd2e7dea8:

#include "ReceiverIR.h"
 
#define LOCK()
#define UNLOCK()
 
#define InRange(x,y)   ((((y) * 0.7) < (x)) && ((x) < ((y) * 1.3)))

/**
 * Constructor.
 *
 * @param rxpin Pin for receive IR signal.
 */

ReceiverIR::ReceiverIR(PinName rxpin, float _speed, PinName _pwma,PinName _pwmb, PinName _ain0, PinName _ain1, PinName _bin0, PinName _bin1)
: evt(rxpin), pwma(_pwma), pwmb(_pwmb),ain1(_ain1),ain0(_ain0), bin0(_bin0),bin1(_bin1) {
    init_state();
    evt.fall(this, &ReceiverIR::isr_fall);
    evt.rise(this, &ReceiverIR::isr_rise);
    evt.mode(PullUp);
    ticker.attach_us(this, &ReceiverIR::isr_wdt, 10 * 1000);    
    Speed_L = 0.95 ;
    //Speed_R = _speed *1.005;
    Speed_R = 0.945 ; 
    init_sp_l = Speed_L;
    init_sp_r = Speed_R;
}
 
/**
 * Destructor.
 */
ReceiverIR::~ReceiverIR() {
}
 
/**
 * Get state.
 *
 * @return Current state.
 */
ReceiverIR::State ReceiverIR::getState() {
    LOCK();
    State s = work.state;
    UNLOCK();
    return s;
}
 
/**
 * Get data.
 *
 * @param format Pointer to format.
 * @param buf Buffer of a data.
 * @param bitlength Bit length of the buffer.
 *
 * @return Data bit length.
 */
int ReceiverIR::getData(RemoteIR::Format *format, uint8_t *buf, int bitlength) {
    LOCK();
 
    if (bitlength < data.bitcount) {
        UNLOCK();
        return -1;
    }
 
    const int nbits = data.bitcount;
    const int nbytes = data.bitcount / 8 + (((data.bitcount % 8) != 0) ? 1 : 0);
    *format = data.format;
    for (int i = 0; i < nbytes; i++) {
        buf[i] = data.buffer[i];
    }
    
    move(buf);
    init_state();
    
    UNLOCK();
    return nbits;
}
 
void ReceiverIR::move(uint8_t *buf){
    char buffer[64];
    sprintf(buffer, "%02X%02X%02X%02X", buf[0], buf[1], buf[2], buf[3]);
    //start
//    if(!strncmp(buffer, "00FF09F6",8)){
//        //pc.printf("forward!\r\n");
//        start = 1;
//        //forward();      
//    }    
    // forward
    if(!strncmp(buffer, "00FF18E7",8)){
        //pc.printf("forward!\r\n");
        forward();      
    }
    // back
    if(!strncmp(buffer, "00FF52AD",8)){
        //pc.printf("back!\r\n");       
        backward();
    }
    // left
    if(!strncmp(buffer, "00FF08F7",8)){
        //pc.printf("left!\r\n"); 
        left();      
    }
    // right
    if(!strncmp(buffer, "00FF5AA5",8)){
        //pc.printf("right!\r\n");
        right();       
    }
    // stop
    if(!strncmp(buffer, "00FF0CF3",8)){
        speedup_l();
    }   
    if(!strncmp(buffer, "00FF5EA1",8)){
        speedup_r();
    }   
    if(!strncmp(buffer, "00FF42BD",8)){
        speeddown_l();
    }   
    if(!strncmp(buffer, "00FF4AB5",8)){
        speeddown_r();
    }   
    if(!strncmp(buffer, "00FF1CE3",8)){
        stop();
    }
    if(!strncmp(buffer, "00FF45BA", 8)){
        cal_start = 1;        
    }
    if(!strncmp(buffer, "00FF47B8", 8)){
        cal_stop = 1;
    }       
}

void ReceiverIR::speedup_l(void){
    //Speed_L += 0.0005; 
//    if(Speed_L > 0.5){
//        Speed_L = 0.5;
//    }
//    pwma = Speed_L;
    kp += 0.5;
}
void ReceiverIR::speeddown_l(void){
    //Speed_L -= 0.0005;
//    if(Speed_L < 0){
//        Speed_L = 0;
//    }
//    pwma = Speed_L;
    kp -= 0.5;
}
void ReceiverIR::speedup_r(void){
   // Speed_R += 0.0005; 
//    if(Speed_R > 0.5){
//        Speed_R = 0.5;
//    }
//    pwmb = Speed_R;

    //max += 5;
    kd += 1;
}
void ReceiverIR::speeddown_r(void){
//    Speed_R -= 0.0005;
//    if(Speed_R < 0){
//        Speed_R = 0;
//    }
//    pwmb = Speed_R;
    
    //max -= 5;
    kd -= 1;
}

void ReceiverIR::speed_l(float sp_l){
    Speed_L = sp_l;
    if(Speed_L > 0.6){
        Speed_L = 0.6;
    }
    pwma = Speed_L;
}

void ReceiverIR::speed_r(float sp_r){
    Speed_R = sp_r;
    if(Speed_R > 0.6){
        Speed_R = 0.6;
    }
    pwmb = Speed_R;
}

void ReceiverIR::forward(void){
    pwma = Speed_L;
    pwmb = Speed_R;

    ain0 = 1;
    ain1 = 0;

    bin0 = 1;
    bin1 = 0;
}
void ReceiverIR::backward(void){
    
    pwma = Speed_L;
    pwmb = Speed_R;

    ain0 = 0;
    ain1 = 1;

    bin0 = 0;
    bin1 = 1;    
}

void  ReceiverIR::left(void){
    pwma = Speed_L*0.8;
    pwmb = Speed_R;

    ain0 = 1;
    ain1 = 0;

    bin0 = 1;
    bin1 = 0;
}
void ReceiverIR::right(void){
    pwma = Speed_L;
    pwmb = Speed_R*0.85;

    ain0 = 1;
    ain1 = 0;

    bin0 = 1;
    bin1 = 0;
}
void ReceiverIR::stop(void){
    
    pwma = 0;
    pwmb = 0;

    ain0 = 0;
    ain1 = 0;

    bin0 = 0;
    bin1 = 0;
}

void ReceiverIR::init_state(void) {
    work.c1 = -1;
    work.c2 = -1;
    work.c3 = -1;
    work.d1 = -1;
    work.d2 = -1;
    work.state = Idle;
    data.format = RemoteIR::UNKNOWN;
    data.bitcount = 0;
    timer.stop();
    timer.reset();
    for (int i = 0; i < sizeof(data.buffer); i++) {
        data.buffer[i] = 0;
    }
}
 
void ReceiverIR::isr_wdt(void) {
    LOCK();
    static int cnt = 0;
    if ((Idle != work.state) || ((0 <= work.c1) || (0 <= work.c2) || (0 <= work.c3) || (0 <= work.d1) || (0 <= work.d2))) {
        cnt++;
        if (cnt > 50) {
            init_state();
            cnt = 0;
        }
    } else {
        cnt = 0;
    }
    UNLOCK();
}
 
void ReceiverIR::isr_fall(void) {
    LOCK();
    switch (work.state) {
        case Idle:
            if (work.c1 < 0) {
                timer.start();
                work.c1 = timer.read_us();
            } else {
                work.c3 = timer.read_us();
                int a = work.c2 - work.c1;
                int b = work.c3 - work.c2;
                if (InRange(a, RemoteIR::TUS_NEC * 16) && InRange(b, RemoteIR::TUS_NEC * 8)) {
                    /*
                     * NEC.
                     */
                    data.format = RemoteIR::NEC;
                    work.state = Receiving;
                    data.bitcount = 0;
                } else if (InRange(a, RemoteIR::TUS_NEC * 16) && InRange(b, RemoteIR::TUS_NEC * 4)) {
                    /*
                     * NEC Repeat.
                     */
                    data.format = RemoteIR::NEC_REPEAT;
                    work.state = Received;
                    data.bitcount = 0;
                    work.c1 = -1;
                    work.c2 = -1;
                    work.c3 = -1;
                    work.d1 = -1;
                    work.d2 = -1;
                } else {
                    init_state();
                }
            }
            break;
        case Receiving:
            if (RemoteIR::NEC == data.format) {
                work.d2 = timer.read_us();
                int a = work.d2 - work.d1;
                if (InRange(a, RemoteIR::TUS_NEC * 3)) {
                    data.buffer[data.bitcount / 8] |= (1 << (data.bitcount % 8));
                } else if (InRange(a, RemoteIR::TUS_NEC * 1)) {
                    data.buffer[data.bitcount / 8] &= ~(1 << (data.bitcount % 8));
                }
                data.bitcount++;
                /*
                 * Set timeout for tail detection automatically.
                 */
                timeout.detach();
                timeout.attach_us(this, &ReceiverIR::isr_timeout, RemoteIR::TUS_NEC * 5);
            }
            break;
        case Received:
            break;
        default:
            break;
    }
    UNLOCK();
}
 
void ReceiverIR::isr_rise(void) {
    LOCK();
    switch (work.state) {
        case Idle:
            if (0 <= work.c1) {
                work.c2 = timer.read_us();
                int a = work.c2 - work.c1;
            } else {
                init_state();
            }
            break;
        case Receiving:
            if (RemoteIR::NEC == data.format) {
                work.d1 = timer.read_us();
            } 
            break;
        case Received:
            break;
        default:
            break;
    }
    UNLOCK();
}
 
void ReceiverIR::isr_timeout(void) {
    LOCK();

    if (work.state == Receiving) {
        work.state = Received;
        work.c1 = -1;
        work.c2 = -1;
        work.c3 = -1;
        work.d1 = -1;
        work.d2 = -1;
    }
    UNLOCK();
}