File content as of revision 5:dace4f3b6e4a:

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

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

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


void Receive(uint8_t Address , char kind,char output[],int num ){
    char order[1] = {kind};
    bool val;
    val = Sensor.write(Address&0xFE, order , 1);
           
    Led[0] = 0;       
    Led[1] = !val;
    wait_ms(1);
    
    val = Sensor.read(Address|1, output, num);// IRデータを受信
    
    Led[0] = !val;
    Led[1] = 0;
    wait_ms(1);
}

void ReceiveFast(uint8_t Address, char kind,  char output[],int num){
    /*初めに相手に何が欲しいのか言わない
     *要は全部よこせということ
     *上位3つだとirのみ
     *上位5つだとirとping
     *
     *[0]:ir num num
     *[1]:ir max_data
     *[2]:ir second_max_data
     *[3]:ping
     *[4]:ping
     **/
    
    bool val;
    Led[0] = 0;
    val = Sensor.read(Address|1,output,num);
    wait_ms(1);
    Led[0] = !val;
    
}


uint8_t IrReceive(uint8_t IrData[]){
    /*
     *Irdata[0] : 1位,2位の場所(1位*13+2位)
     *Irdata[1] : 1位の値
     *return    : 1位の場所
     */
    char data_r[3] = {0},data_l[3] = {0};

    Receive(ADDRESS_R,BALL,data_r,3);
    Receive(ADDRESS_L,BALL,data_l,3);
    
    if((data_r[0] == 125)||(data_l[0] == 125)){/*ボールを検知しているかチェック*/
        if((data_r[0] == 125)&&(data_l[0] == 125)){
            IrData[0] = 12*13 + 12;
            return 12;
        }
        if(data_r[0] == 125){
            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);      
    }
    
    
}
uint8_t IrReceiveFast(){
    /*
     *return    : 1位の場所
     */
    char data_r[3] = {0},data_l[3] = {0};

    ReceiveFast(ADDRESS_R,BALL,data_r,3);
    ReceiveFast(ADDRESS_L,BALL,data_l,3);
    
    if((data_r[0] == 125)||(data_l[0] == 125)){/*ボールを検知しているかチェック*/
        if((data_r[0] == 125)&&(data_l[0] == 125)){
        
            return 12;
        }
        if(data_r[0] == 125){
            
            return (data_l[0]/6)+6;    
        }
        
        return data_r[0]/6;
    }
    
    if(data_r[1]<data_l[1]){
        
        return data_r[0]/6;
        
    }else{
        
        return (data_l[0]/6+6);      
    }
    
    
}


void PingReceiveRL(char ping[]){
    /*
     * ping[0] : Right ping data
     * ping[1] : Left  ping data
     */
    Receive(ADDRESS_R,SONIC,ping,2);
    
}

void PingReceiveFB(char ping[]){
    /*
     * ping[0] : FRONT ping data
     * ping[1] : BACK  ping data
     */
    Receive(ADDRESS_L,SONIC,ping,2);
    
}


void move(int vr,int vl, double vs ,int Rad){
    double pwm[4] = {0};
    uint8_t i = 0;
    pwm[0] = vr - vs;
    pwm[1] = 0;
    pwm[2] = 0;
    pwm[3] =  vl + vs;

    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];
    }
    SetRad = Rad;
}
void fool (int *Rad, int *Power){
    static int Last_Rad = 0;
    static int Last_Vector = 1;
    int rad = *Rad;
    int Temp;
    Temp = Last_Rad % 180;

    if((Temp>70) &&(Temp<110)){
        Temp = *Rad % 180;
        if((Temp>70) &&(Temp<110)){
            Temp = abs(*Rad - Last_Rad);
            if(Temp>160){
                Last_Vector = -1 * Last_Vector;//正転逆転切り替え
                if(*Rad/180){
                    *Rad = Angle[Last_Rad%180] -(Last_Rad - *Rad%180);
                    *Power = *Power * Last_Vector;
                }else{
                    *Rad = Angle[Last_Rad%180] -(Last_Rad%180 - *Rad);
                    *Power = *Power * Last_Vector;
                }
                Last_Rad = rad;
                return;
            }else if((Last_Vector+2) == 1){
                /*逆転のまま角度拡張*/
                if(*Rad/180){
                    *Rad = -360 + *Rad ;
                    *Power = *Power * Last_Vector;
                }else{
                    *Power = *Power * Last_Vector;
                }
                Last_Rad = rad;
                return;

            }else if((Last_Vector+2) == 3){
            /*正転のまま*/
                if(*Rad/180){
                    *Rad = -360 + *Rad ;
                    *Power = *Power * Last_Vector;
                }else{
                    *Power = *Power * Last_Vector;
                    Last_Rad = rad;
                }
                Last_Rad = rad;
                return;
            }
        }
    }
    /*通常動作*/
    Last_Vector = RadToVector[(*Rad-1)/90];
    *Rad = Angle[*Rad%180];
    *Power = *Power * Last_Vector;
    Last_Rad = rad;

}

void IrFrontAction(int *CompassDef , uint8_t IrMemo[],double ReV)//ball 12 or 0 o-clock
{
    
}

void IrBackAction(int *CompassDef , uint8_t IrMemo[],double ReV)//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[],double ReV)//Ball is not found.
{
    //止まっとく
    S555.position(0.0);
    move(0,0,ReV,0);

    /*line検知をしないバージョン*/
    /*
    char Ping[2] = {0};

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

void PidUpdate(int CompassDef , double SetAngle, double *Pid_up)
{ 
    int  Compass;
    double inputPID = 0.0;
    
    Compass = ((hmc6352.sample() / 10) + 540 - CompassDef) % 360;
    wait_ms(10);
    pid.setSetPoint((int)((REFERENCE + SetAngle) / 1.0)); 
    inputPID = Compass;
    
    pid.setProcessValue(inputPID);
    *Pid_up =  -(pid.compute());
    
}
uint8_t SwRead(){
/******
 *retrun : sw_state
 *StartS = 0x01;
 *Debug2 = 0x02;
 *Debug1 = 0x04;
 *Debug3 = 0x06;
 *Kicker = 0x08;
 *Calibration = 0x10;

 *
 *****/
    uint8_t i,temp,temp2;
    temp = ~Sw - 224;
    if(!(temp == Calibration
       ||temp == Kicker
       ||temp == Debug1
       ||temp == Debug2
       //||temp == Debug3
       ||temp == StartS)) return 0;/*スイッチが押されていない*/
    if(!(temp == 0x00)){
        for(i = 0; i < 50; i++);
        temp2 = ~Sw - 224;
        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());
    S555.position(SetRad);
}

void SetUp(int *CompassDef){
    /*初期化*/
    double ReV;
    int i;
    Kick = 0;
    Sw.mode(PullUp);
    
    pid.setInputLimits(MINIMUM,MAXIMUM);            //pid sed def
    pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);     //pid sed def
    pid.setBias(PID_BIAS);                          //pid sed def
    pid.setMode(AUTO_MODE);                         //pid sed def
    pid.setSetPoint(REFERENCE);                     //pid sed def  
    
    Motor.baud(115200);                             //ボーレート設定
    Motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
    Motor.attach(&tx_motor,Serial::TxIrq);          //送信空き割り込み（モータ用）
    

    S555.calibrate(0.0005, 120.0);
    S555.position(0.0);    //初期位置にセット
    move(0,0,0,0);//停止
    Sensor.frequency(100000);
    hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    *CompassDef = (hmc6352.sample() / 10);
    for(i = 0;i<15;i++){
      PidUpdate(*CompassDef,0,&ReV);
    }
    
    Lcd.printf("%f\n",ReV);
    wait_ms(100);
    
}
void StartLoop(int *CompassDef){
    /*
     *スイッチが押されるまでロボットはスタートしない．
     *
     *switch割り当て
     *1.コンパスのキャリブレーション実行スイッチ
     *2.キッカーのキック(check用)
     *3,4.自由
     *5.StartSw
     */
     uint8_t State = 0;
     int Compass = 0;
     uint8_t IrData[3] = {0};
     uint8_t Irnum = 0;
     uint8_t LineData = 0;
     while(1){
        Led[0] = Led[1] = Led[2] = Led[3] = 1;
        //Lcd.cls();
        State = SwRead();
        if(State == 0) continue;
        
        if(State == StartS){
            /*loop end & start*/ 
           return;
        }
        
        
        if(State == Debug1){
            while((State == Debug1)){
                /*Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
                Lcd.printf("%d\n",Compass);
                */
                
                
                LineData = (~Line+0x00) & 0x0F;
                Lcd.printf("%d\n",LineData);
                
                wait_ms(100); 
                State = SwRead();
                
            }   
            
            Lcd.cls(); 
            continue;
            /* debug command free  
             *
             * decide movement of the beginning.  
             *
             **/
        }
        if(State == Debug2){
             while((State == Debug2)){
                Irnum = IrReceive(IrData);
                Lcd.printf("%d\n",Irnum);
                wait_ms(100);
                State = SwRead();
             }
             Lcd.cls();
             continue;
            
            /* debug command free  
             *
             *display out to selected 3 menus. compass, ir, ping, line,etc
             *
             **/   
        }
        /*
        if(State == Debug3){
             while((State == Debug3)){
                Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
                Lcd.printf("%d\n",Compass);
                State = SwRead();
                wait_ms(100);
             }
             Lcd.cls();
            */
            /* debug command free  
             *
             *display out to selected 3 menus. compass, ir, ping, line,etc
             *
             **/   
        //}
        
        if(State == Kicker){
            while((State == Kicker)){
                Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360;
                Lcd.printf("%d\n",Compass);
                wait_ms(100); 
                State = SwRead();
                
            }   
            Lcd.cls(); 
            /*kicker check */
            /*
            Led[4] = 1;
            Kick = 1;
            wait(0.5);
            Kick = 0;
            Led[4] = 0;
            wait(1);
            */
            continue;
        }
        if(State == Calibration){
            Led[0] = Led[1] = Led[2] = 0;
            move(20,20,0,0);
            while((State == Calibration)){
                wait_ms(100); 
                State = SwRead();
                
            }   
            move(0,0,0,0);
            /*
            hmc6352.setCalibrationMode(ENTER);
            while((State == Calibration)){
                State = SwRead();
            }    
            hmc6352.setCalibrationMode(EXIT);
            wait(0.3);//必要
            Led[3] = 0;
            */
            /*calibration command enter*/ 
            continue;   
        }
    }     
    
}
int main() {
    
    /*Ir*/
    uint8_t IrNum = 12;//場所によるirの数を表したもの0~11まではボールがある状態12はボールがない状態
    //uint8_t IrNumOld = 0;//過去値
    uint8_t IrData[2];//0:1位*13+2位,1:1位の値

    /*Line*/
    uint8_t LineData = 0;
    
    /*Compass*/
    int CompassDef = 0;
    //int Compass = 0;
    double SetAngle = 0; 

    /*PID補正move加算値 Revise */
    double ReV = 0.0;
    int Rad = 0;
    
    /*State */ 
    //Direction LineIr = NA;//方位設定奴...エラーでてめんどくさいので後でまたやることにする。
    uint8_t LineIr = 0;
    uint8_t IrChange[13] ={0x01,0x01,0x02,0x02,0x02,0x04,
                            0x04,0x04,0x08,0x08,0x08,0x01,0x00};

    int Power = 0;

    /*関数ポインタ*/
    void (*AnotherAction[3])(int *, uint8_t [],double);
    AnotherAction[0] = IrFrontAction;
    AnotherAction[1] = IrBackAction;
    AnotherAction[2] = GoHome;
    
    /*check 変数*/
    double s_deg[13] = { 0.0,-30.0,-60.0,-90.0, 60.0, 30.0, 0.0,-30.0,-60.0, 90.0, 60.0, 30.0, 0.0};

    int vr[13] = { 20,20,20,20,-20,-20,-20,-20,-20,20,20,20,0};
    
    int vl[13] = { 20,20,20,20,-20,-20,-20,-20,-20,20,20,20,0};
   
    
    SetUp(&CompassDef);/*set up routine*/
    StartLoop(&CompassDef); /*loop strat, switch push end*/
    
    while(1){
         
         Led[0] = 1;
       
        /*白線を読んでいないか確認する*/
        
        LineData = (~Line+0x00) & 0x0F; 
        if(LineData){
            //IrNum = IrReceiveFast();
            //LineIr = LineData & IrChange[IrNum];    //一箇所でも一致すればlineを検知している．
            while(LineData){
                move(0,0,0,0);//
                Led[1] = Led[2] = Led[3] = 1;  
                LineData = (~Line+0x00) & 0x0F;
                //IrNum = IrReceiveFast();
                wait_ms(10);
                //LineIr = LineData & IrChange[IrNum];//一箇所でも一致すればlineを検知している．
            
            }
            Led[1] = Led[2] = Led[3] = 0;
            //wait(0.02);
            
        }
        
        Led[2] = 1;
        IrNum = IrReceive(IrData);
        wait_ms(100);
        Led[2] = 0;
        //PidUpdate(CompassDef ,SetAngle,&ReV);
        Led[3] = 1;
        Power = 20;
        Rad = 359-IrNum*30;
        fool(&Rad,&Power);
        move(Power,Power,ReV,Rad);
        wait_ms(100);
        Led[3] = 0;
        
        Led[2] = 1;
        wait_ms(200);
       
        Led[0] = Led[1] = Led[2] = Led[3] = 0;

    }
    
    /*
    while(0){
    //デモプログラム
    S555.position(0.0);
    
    wait(1.5);
    S555.position(90.0);

    }
    
    */
    
    
}