File content as of revision 2:de6958cb7072:

/***********************************
 *RoboCupJunior Soccer B Open 2014 
 *Koshinnestu progrum
 *
 *　
 *　データからロボットの移動やキッカー等のモータの動作を決定する処理を行う
 *
 *　MotorDriverにmaxonに命令
 *　
 *　servoにステアリング指示
 *　
 *　LCDでデバック
 *
 *　スイッチ4つとスタートスイッチで処理を実行
 *  

 ************************* 
 *  Pin Map 
 *  
 *  p5~p8   >> InterruptIn >> LineSensor 
 *
 *  p9,p10  >> I2C         >> LPC1114FN28/102 read
 *
 *  p13,p14 >> Serial      >> Motor
 * 
 *  p21     >> PwmOut      >> Servo 
 *
 *  p22~p26 >> DigitalIn   >> DebugSw and StartSw 
 *
 *  p27,p28 >> I2C         >> DebugLCD
 *
 *  p29     >> DigitalOut  >> Kicker 
 *    
 *  **now , never use pin number p11,p12,p15,p16,p17,p18,p19,p20, p29,p30
 *
 *  will use kicker, any Pin set in DigitalOut  soon.
 *   
 *  
 ******************************/






#include "mbed.h"
#include "PID.h"
#include "AQM0802A.h"
#include "HMC6352.h"
#include "Servo.h"
#include "main.h"
#include <math.h>
#include <sstream>

/*回り込みの計算用*/
#define PI 3.1415926

/*LPC1114 I2C Slave Address*/
#define ADDRESS_R 0xAA
#define ADDRESS_L 0xC0

/*BusIn sw　入力値*/
#define Calibration 0x01
#define Kicker 0x02
#define Debug1 0x04
#define Debug2 0x08
#define StartS 0x10

/*send to LPC1114 ,receive data choice*/
#define READ_IR    0x01 
#define READ_PING  0x02




BusIn   Sw(p22,p23,p24,p25,p26);
BusIn   Line(p5,p6,p7,p8);//No reading line → 0b1111 ,not pull up
DigitalOut  Led[4] = {LED1,LED2,LED3,LED4};
DigitalOut  Kick(p29);

Servo       S555(p21);
I2C         Sensor(p9,p10);//sda,scl
HMC6352     hmc6352(p9,p10);//sda,scl
AQM0802A    Lcd(p28,p27);//sda,scl
Serial      Motor(p13,p14);//tx,rx
Serial      pc(USBTX,USBRX);

extern string StringFIN;
extern void array(int,int,int,int);

int speed[4] = {0};            

typedef enum {FRONT = 1, RIGHT, BACK, LEFT, NA} Direction;



uint8_t IrReceive(uint8_t IrData[]){
    /*
     *Irdata[0] : 1位,2位の場所(1位*13+2位)
     *Irdata[1] : 1位の値
     *return    : 1位の場所
     */
    char data_r[3],data_l[3];
    bool val;
    Sensor.write(READ_IR);//一斉送信のつもり
    
    val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信
    Led[0] = val;
    wait_ms(5);
    val = Sensor.read(ADDRESS_L|1, data_l, 3);
    Led[0] = !val;
    
    if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/
        if((data_r[0] == 0)&&(data_l[0] == 0)){
            IrData[0] = 12*13 + 12;
            return 12;
        }
        if(data_r[0] == 0){
            IrData[0] = (data_l[0]/6+6)*13 + (data_l[0]%6+6);  
            IrData[1] = data_l[1];
            return data_l[0]/6+6;
            
        }
        
        IrData[0] = (data_r[0]/6)*13 + (data_r[0]%6);  
        IrData[1] = data_r[1];
        return data_r[0]/6;
    }
    
    if(data_r[2]<data_l[2]){
        if(data_r[2]<data_l[1]){   
            IrData[0] = (data_r[0]/6)*13 + (data_r[0]%6);
            IrData[1] = data_r[1];
            return data_r[0]/6;
        }
         
        IrData[0] = (data_r[0]/6)*13 + (data_l[0]/6+6);
        IrData[1] = data_r[1];
        return data_r[0]/6;
        
    }else{
        if(data_l[2]<data_r[1]){   
            IrData[0] = (data_l[0]/6+6)*13 + (data_l[0]%6+6);
            IrData[1] = data_l[1];
            return (data_l[0]/6+6);
        }
         
        IrData[0] = (data_l[0]/6+6)*13 + (data_r[0]/6);
        IrData[1] = data_l[1];
        return (data_l[0]/6+6);      
    }
    
    
}

void PingReceiveRL(char ping[]){
    /*
     * ping[0] : Right ping data
     * ping[1] : Left  ping data
     */
    char ReadSelect[1] = {READ_PING};
    bool val;
    val = Sensor.write(ADDRESS_R|0, ReadSelect , 1);
    Led[2] = val;
    val = Sensor.read(ADDRESS_R|1, ping, 2);
    Led[2] = !val;
    
}

void PingReceiveFB(char ping[]){
    /*
     * ping[0] : FRONT ping data
     * ping[1] : BACK  ping data
     */
    char ReadSelect[1] = {READ_PING};
    bool val;
    val = Sensor.write(ADDRESS_L|0, ReadSelect , 1);
    Led[2] = val;
    val = Sensor.read(ADDRESS_L|1, ping, 2);
    Led[2] = !val;
    
    
}



void move(int vr, int vl){
    /**
     * pwm[1] : right wheel(+data = forward/ -data = backward)
     * pwm[2] : left  wheel(+data = forward/ -data = backward)
     * pwm[3] : not used
     * pwm[4] : not used
     *
     */
    
    double pwm[4] = {0};
    uint8_t i = 0;
    pwm[0] = -vr;
    pwm[1] = 0;
    pwm[2] = 0;
    pwm[3] = vl;

    for(i = 0; i < 4; i++){
            if(pwm[i] > 100){
                pwm[i] = 100;
            }else if(pwm[i] < -100){
                pwm[i] = -100;
            }
        speed[i] = pwm[i];
    }
}


void IrFrontAction(int *CompassDef , uint8_t IrMemo[])//ball 12 or 0 o-clock
{
/*前にボールがある場合の動作*/
    int Compass;
    char Ping[2];
    uint8_t Line,LineIr;
    unsigned int IrNumMax; 
    
    
    if(IrReceive(IrMemo)<=150){//適当な150
        /*ステッピングを正面に設定*/
        /*busy_wait()*/
        /*モーターを前進*/
        return;
    }
    PingReceiveRL(Ping);
    if((Ping[0]>90)&&(Ping[1]>40)){//数値は適当
        /*ステッピングを少しだけ傾ける*/
        /*モーターを左右差つけて回す(ロボットは少し傾く)*/
        /*busy_wait()*/
        /*モーターを前進*/
        while(1){
        
            if(Line){ //ラインを検知していないか
                //LineIr = Line & (IrReceiveS()/3 + 1);//計算により方位を合わせる。
                while(LineIr){
                    move(0,0);//
                    Led[1] = Led[2] = Led[3] = 1;  
                
                    //LineIr = LineReceive() & (IrReceiveS()/3 + 1);
                }
                Led[1] = Led[2] = Led[3] = 0;
                
                Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
                while(!((Compass>=150)&&(Compass<=210))){
                    move(20,-20);//適当な回転。
                    Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
                }
                return;
            }
            Kick = 1;
            wait_ms(200);
            Kick = 0;
            
            //IrNumMax = IrReceiveSM();//位置と大きさ
            
            if(!((IrNumMax&0x00)>>8) == 1){//この1はボールの関数の1を表す.
                Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
                while(!((Compass>=150)&&(Compass<=210))){
                    move(20,-20);//適当な回転。
                    Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
                }
                return;
            }    
            
            if((IrNumMax&0x00FF)<150){
                Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
                while(!((Compass>=150)&&(Compass<=210))){
                    move(20,-20);//適当な回転。
                    Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
                }
                return;
            }                    
            
        }
        
    }else if((Ping[0]>40)&&(Ping[1]>90)){
        
    }else{
        
    }
        
}

void IrBackAction(int *CompassDef , uint8_t IrMemo[])//ball found six o-clock
{
    /***
     * 6時にボールがある場合の処理．右と左のスペースを確認して広い方から回り込む
     *
     **/
     char Ping[2];
     
     PingReceiveRL(Ping);
     
    
    if(IrMemo[0] == 6){//check ir number
        return ;
    }
    if(Ping[0]<Ping[1]){
        /*右が大きい*/
        
        
        return;
    }
    /*左が大きい*/
    
    
    
    
    }
void GoHome(int *CompassDef , uint8_t IrMemo[])//Ball is not found.
{
    /*line検知をしないバージョン*/
    int Compass;
    char Ping[2] = {0};

     
    PingReceiveFB(Ping);
    if(Ping[1] >=60){//後ろからの距離60cm
        move(-20,-20);
        PingReceiveFB(Ping);
        return ;
    } 
        
    move(0,0);//stop
    
}


uint8_t SwRead(){
/******
 *retrun : sw_state
 *Calibration = 0x01;
 *Kicker = 0x02;
 *Debug1 = 0x04;
 *Debug2 = 0x08;
 *StartS = 0x10;
 *
 *****/
    uint8_t i,temp,temp2;
    temp = Sw;
    if(!(temp == Calibration
       ||temp == Kicker
       ||temp == Debug1
       ||temp == Debug2
       ||temp == StartS)) return 0;/*スイッチが押されていない*/
    if(temp !=0x00){
        for(i = 0; i < 50; i++);
        temp2 = Sw;
        if(temp == temp2){
            return temp;
        }
    }
    return 0;
}

//通信（モータ用）
void tx_motor(){
    array(speed[0],speed[1],speed[3],speed[2]);
    Motor.printf("%s",StringFIN.c_str());
}

void SetUp(int *compass_def){
    /*初期化*/
    
    Kick = 0;
    Sw.mode(PullUp);
    
    Motor.baud(115200);                             //ボーレート設定
    Motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
    Motor.attach(&tx_motor,Serial::TxIrq);          //送信空き割り込み（モータ用）
    //move(0,0);//停止

    S555.calibrate(0.0005, 120.0);
    S555.position(0.0);    //初期位置にセット
    
    hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    *compass_def = (hmc6352.sample() / 10);
    
    //Lcd.cls();
    
}
void StartLoop(){
    /*
     *スイッチが押されるまでロボットはスタートしない．
     *
     *switch割り当て
     *1.コンパスのキャリブレーション実行スイッチ
     *2.キッカーのキック(check用)
     *3,4.自由
     *5.StartSw
     */
     uint8_t State = 0;
     
     while(1){
         
        State = SwRead();
        if(State == 0) continue;
        
        if(State == Calibration){
            /*calibration command enter...
             *
             *
             */ 
            continue;   
        }
        if(State == Kicker){
            /*
             *kicker check 
             *
             *
             */
            continue;
        }
        if(State == Debug1){
            /* debug command free  
             *
             *display out to selected 3 menus. compass, ir, ping, line,etc... 
             *
             **/
            
        }
        if(State == Debug2){
            /* debug command free  
             *
             * decide movement of the beginning.  
             *
             *
             **/
            
        }
        if(State == StartS){
            /*game start...*/ 
           return;
        }
        
    }     
    
}
int main() {
    
    /*Ir*/
    uint8_t IrNum;//場所によるirの数を表したもの0~11まではボールがある状態12はボールがない状態
    uint8_t IrData[2];//0:1位*13+2位,1:1位の値
    /*Line*/
    uint8_t LineData = 0;
    
    /*Compass*/
    int CompassDef = 0, Compass = 0;
    hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    
    /*State */ 
    //Direction LineIr = NA;//方位設定奴...エラーでてめんどくさいので後でまたやることにする。
    uint8_t LineIr = NA;
    uint8_t IrChange[13] ={1,1,1,2,2,2,4,4,4,8,8,8};

    /*関数ポインタ*/
    void (*AnotherAction[3])(int *, uint8_t []);//irの位置による分岐
    AnotherAction[0] = IrFrontAction;
    AnotherAction[1] = IrBackAction;
    AnotherAction[2] = GoHome;
    
    
    SetUp(&CompassDef);/*set up routine*/
    //StartLoop(); /*loop stat, switch push end*/
    
    int val = 0;
    int compassy;
    char data[3] = {0};
    char order[1] = {0};
    while(1) {
        
        
        
        /*白線を読んでいないか確認する*/
        /*
        LineData = (~Line+0x00) & 0x0F; 
        if(LineData){
            IrNum = IrReceive(IrData);
            LineIr = LineData & IrChange[IrNum];    //一箇所でも一致すればlineを検知している．
            while(LineIr){
                move(0,0);//
                Led[1] = Led[2] = Led[3] = 1;  
                LineData = (~Line+0x00) & 0x0F;
                IrNum = IrReceive(IrData);
                LineIr = LineData & IrChange[IrNum];//一箇所でも一致すればlineを検知している．
            }
            Led[1] = Led[2] = Led[3] = 0;
            continue;
        }
        */
        
        
        
        /*ロボットの向いている向きが正面か確認する*/
        /*
        Compass = ((hmc6352.sample() / 10) + 540 - CompassDef) % 360;    
        if(!((Compass>=150)&&(Compass<=210))){
            while(!((Compass>=150)&&(Compass<=210))){
                move(20,-20);
                Compass = ((hmc6352.sample() / 10) + 540 - CompassDef) % 360;
            }
            continue;
        
        }
        */
        /*ボールの位置を知る*/
        //IrNum = IrReceive(IrData);
        
        order[0] = 0x00;
        for(int i=0; i<3; i++) data[i]=100;
        
        val = Sensor.write(ADDRESS_R&0xFE, order , 1);
           
        Led[0] = 0;       
        Led[1] = !val;
        
        wait_ms(.25);
        
        val = Sensor.read(ADDRESS_R|1, data, 3);// IRデータを受信
        
        Led[0] = !val;
        Led[1] = 0;
        
        wait_ms(.25);
        
        pc.printf("num = %d irData1 = %d irData2 = %d\n",(int)data[0],(int)data[1],(int)data[2]);
        
        for(int i=0; i<3; i++) data[i]=100;
        
        
        
        
        
        
        
        
        val = Sensor.write((ADDRESS_R+2)&0xFE, order , 1);
           
        Led[0] = 0;       
        Led[1] = !val;
        
        wait_ms(.25);
        
        val = Sensor.read((ADDRESS_R+2)|1, data, 3);// IRデータを受信
        
        Led[0] = !val;
        Led[1] = 0;
        
        wait_ms(.25);
        
        pc.printf("num = %d irData1 = %d irData2 = %d\n",(int)data[0],(int)data[1],(int)data[2]);
        
        for(int i=0; i<3; i++) data[i]=100;
        
        
        
        
        
        //pc.printf("Heading is: %03d\n",(int)((hmc6352.sample()/10.0)+0.5));
        
        compassy = (
                        (
                            ((int)(hmc6352.sample()/10.0))
                            + 540
                            - CompassDef
                        )%360
                    );
        
        Lcd.cls();
        Lcd.printf("Hello\n %d", compassy );
        
        wait_ms(.25);
        
        for(int i=0; i<3; i++) data[i]=100;
        
        
        /*複雑な処理が必要な箇所については関数に飛ばす*/
        /*
        
        if(IrNum == 0 || IrNum == 6||IrNum == 12){
            (*AnotherAction[IrNum/6])(&CompassDef,IrData); //Front OR Back Action
            continue;
        }
        
        */
        
        /*残りの箇所についての処理を行う*/    
        
        
        
        //wait_ms(0);
    
    
    }
    
    
    while(0){
    /*デモプログラム*/

        
    }
    

    
    
}