テスト

Dependencies:   RemoteIR TB6612FNG2 mbed

Fork of RemoteIR_TestProgram by Shinichiro Nakamura

main.cpp

Committer:
mbed_Cookbook_SE
Date:
2015-12-23
Revision:
6:9bddde38e25e
Parent:
5:40750d5779ca

File content as of revision 6:9bddde38e25e:

#include "mbed.h"

#include "ReceiverIR.h"
#include "TransmitterIR.h"
#include "TB6612.h"

ReceiverIR ir_rx(p14);
TransmitterIR ir_tx(p23);
TB6612 MOTOR_A(p21,p19,p20);            // PWM IN1 IN2
TB6612 MOTOR_B(p22,p29,p30);            // PWM IN1 IN2

int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout = 100) {
    int cnt = 0;
    while (ir_rx.getState() != ReceiverIR::Received) {
        cnt++;
        if (timeout < cnt) {
            return -1;
        }
    }
    return ir_rx.getData(format, buf, bufsiz * 8);
}

int transmit(RemoteIR::Format format, uint8_t *buf, int bitlength, int timeout = 100) {
    int cnt = 0;
    while (ir_tx.getState() != TransmitterIR::Idle) {
        cnt++;
        if (timeout < cnt) {
            return -1;
        }
    }
    return ir_tx.setData(format, buf, bitlength);
}

int SearchCode( uint8_t *code ,uint8_t **buf , int size)
{
    int ret = -1;
    for(int i=0;i<5;i++)
    {
        if(memcmp( code , buf[i] , size ) == 0 ){
            ret = i;
            break;
        }
    }
    return(ret);
}

void RegistrationCode(uint8_t *forward ,uint8_t *back ,uint8_t *left ,uint8_t *right ,uint8_t *stop)
{
    uint8_t buf[32];
    int bitlength;
    RemoteIR::Format format;
    uint8_t *ir_buf[5] = {forward,back,left,right,stop};
    
    memset(buf, 0x00, sizeof(buf));
    
    for(int i=0;i<5;i++)
    {
        bitlength = -1;
        while(bitlength < 0)
        {
            bitlength = receive(&format, buf, sizeof(buf));
            if (bitlength < 0) {
                continue;
            }
            
            if( SearchCode(buf,ir_buf,sizeof(buf)) != -1 )
            {
                bitlength = -1;
                continue;
            }

            memcpy( ir_buf[i] , buf , sizeof(buf));
        }
    }
}

int main(void) {
    
    uint8_t forward[32],back[32],left[32],right[32],stop[32];
    uint8_t *ir_buf[5] = {forward,back,left,right,stop};

    RegistrationCode( forward ,back  ,left ,right ,stop);

    while (1) {
        uint8_t buf[32];
        int bitlength1;
        RemoteIR::Format format;

        memset(buf, 0x00, sizeof(buf));

        bitlength1 = receive(&format, buf, sizeof(buf));
        if (bitlength1 < 0) {
            continue;
        }
        switch(SearchCode(buf,ir_buf,sizeof(buf)))
        {
        case 0: // forward
            MOTOR_A = 100;
            MOTOR_B = 100;
            break;
        case 1: // back
            MOTOR_A = -100;
            MOTOR_B = -100;
            break;
        case 2: // left
            MOTOR_A = 100;
            MOTOR_B = -100;
            break;
        case 3: // right
            MOTOR_A = -100;
            MOTOR_B = 100;
            break;
        case 4: // stop
            MOTOR_A = 0;
            MOTOR_B = 0;
            break;
        default:
            break;
        }
    }
}