mbed_pro_miniを使った相撲ロボットプログラムの雛形

Dependencies:   RemoteIR USBDevice mbed

main.cpp

Committer:
hardtail
Date:
2015-09-10
Revision:
0:caf66b280785

File content as of revision 0:caf66b280785:

#include "mbed.h"
#include "USBSerial.h"
#include "ReceiverIR.h"

#define LED1 P0_29
#define LED2 P0_28
#define LED3 P0_27
#define LED4 P0_26

#define front 1
#define back 2
#define left 3
#define right 4
#define breaking 5

#define IRrecerver P0_5

/*******X-box rimote controller********/
#define Xbutton     1
#define powoer      2
#define startBTN    3
#define deside      4
#define windows     5
/**************************************/

//USBSerial pc;

DigitalOut led[] =
    {DigitalOut(LED1),
    DigitalOut(LED2),
    DigitalOut(LED3),
    DigitalOut(LED4)};
    
DigitalOut myled(P0_25);
    
DigitalOut motorR[] = {DigitalOut(P0_0), DigitalOut(P0_1)};
    
DigitalOut motorL[] = {DigitalOut(P0_2), DigitalOut(P0_3)};

DigitalIn sensor[] = {
    DigitalIn(P0_10),
    DigitalIn(P0_11),
    DigitalIn(P0_12),
    DigitalIn(P0_13),
    DigitalIn(P0_15)};
    
DigitalIn dip[] = {
    DigitalIn(P0_9),
    DigitalIn(P0_8),
    DigitalIn(P0_7),
    DigitalIn(P0_6)};
    
DigitalIn LineSensor[] = {DigitalIn(P0_17),DigitalIn(P0_15),DigitalIn(P0_20),DigitalIn(P0_23)};

ReceiverIR ir_rx(IRrecerver);
RemoteIR::Format format;

int IRswitch();
int IRstop();
void IRclear();

Ticker motorTest;
int mTestStatus = 0;

Timer modeCnt;
Timer startCnt;
Timer rollingCnt;

void go (int);
void flip();
void EnemySensWait(int);

int nowMode = 1;
int waitFlag = 0;
int fightingMode = 0;
int enemySensorEnble = 0;
int startFlag = 0;
int goFlag = 0;
int startSelect = 3;
int enemySensActive = 1;

int main() {
    //motorTest.attach(&flip, 1.0);
    
    for(int i=0; i< 5; i++) sensor[i].mode(PullUp);
    for(int i=0; i< 4; i++) dip[i].mode(PullDown);
    
    for(int i=0;i<4; i++)LineSensor[i].mode(PullUp);
    LineSensor[2].mode(PullUp);
    
    int stop = 1;
    
    while(stop){
        if(IRswitch() == Xbutton) stop = 0;
        wait(0.05);
    }
    
    wait(5);
    
    int start = 1;
    
    
    while(start){
        
        go(front);
        
        //if(LineSensor[] == )
            
        if(IRswitch() == powoer) start = 0;    
    }
    
    while(1) {
        //for(int i=0; i< 4; i++) led[i] = sensor[i];
        for(int i=0; i< 4; i++) led[i] = LineSensor[i];
        myled = LineSensor[0];
        /*
        for(int i=0; i< 4; i++) led[i] = dip[i];
        myled = sensor[2];
        
        if(dip[1]) go(front);
        if(dip[2]) go(left);
        if(dip[3]) go(right);
        if(dip[1]==0 && dip[2]==0 && dip[3]==0) go(breaking);
        */
        wait(0.05);
           
    }
}

int IRswitch(){
    uint8_t buf[32];
    int bitcount;
    int res = 0;
    int i;

    res = 0;

    if (ir_rx.getState() == ReceiverIR::Received) {
        bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8);

        //pc.printf("reserve : ");

        if((bitcount > 30)){
#if 0
            for(i=0;i<sizeof(buf);i++){
                pc.printf("%x,",buf[i]);
            }
#endif
            //if(buf[1]==0x80) pc.printf("OK:");
            if(buf[3]==0x24||buf[3]==0x12) {
                //pc.printf("X\n");
                res = Xbutton;
            }
            if(buf[3]==0x20||buf[3]==0x10) {
                //pc.printf("powoer\n");
                res = powoer;
            }
            if(buf[3]==0x30||buf[3]==0x18) {
                //pc.printf("start\n");
                res = startBTN;
            }
            if(buf[3]==0x50||buf[3]==0xa0) {
                //pc.printf("win\n");
                res = windows;
            }
            //pc.printf("\n");
        }
    }

    return res;
}

void IRclear(){
    uint8_t buf[32];

    while (ir_rx.getState() == ReceiverIR::Received){
        ir_rx.getData(&format, buf, sizeof(buf) * 8);
    }
}

void go (int mode){
    switch(mode){
        case front:
            motorR[0] = 0;
            motorR[1] = 1;
            
            motorL[0] = 0;
            motorL[1] = 1;
            
            
            break;
            
        case back:
            motorR[0] = 1;
            motorR[1] = 0;
            
            motorL[0] = 1;
            motorL[1] = 0;
            break;
            
        case left:
            motorR[0] = 0;
            motorR[1] = 1;
            
            motorL[0] = 1;
            motorL[1] = 0;
            break;
            
        case right:
            motorR[0] = 1;
            motorR[1] = 0;
            
            motorL[0] = 0;
            motorL[1] = 1;
            break;
            
        case breaking:
            motorR[0] = 1;
            motorR[1] = 1;
            
            motorL[0] = 1;
            motorL[1] = 1;
            break;
        }
}

void flip(){
    switch(mTestStatus){
        case 0:
            motorR[0] = 1;
            motorR[1] = 1;
            
            motorL[0] = 1;
            motorL[1] = 1;
            mTestStatus = 1;
            break;
        case 1:
            motorR[0] = 1;
            motorR[1] = 0;
            
            motorL[0] = 0;
            motorL[1] = 1;
            mTestStatus = 2;
            break;
        case 2:
            motorR[0] = 0;
            motorR[1] = 1;
            
            motorL[0] = 1;
            motorL[1] = 0;
            mTestStatus = 3;
            break;
        case 3:
            motorR[0] = 0;
            motorR[1] = 0;
            
            motorL[0] = 0;
            motorL[1] = 0;
            mTestStatus = 0;
            break;
        }
}

void EnemySensWait(int rollTime){
    rollingCnt.reset();
    rollingCnt.start();
    while(sensor[2] == 1 && rollingCnt.read_ms() < rollTime);
    rollingCnt.stop();
}