File content as of revision 7:01bb0b0a98f1:

/***********************************
 *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 "Servo.h"
#include "PID.h"
#include "AQM0802A.h"
#include "main.h"


void ServoWait(){
    WaitFlag = 0;
}


void move(int vr,int vl, double vs ,int Degree){
    double pwm[4] = {0};
    uint8_t i = 0;
    static int last_deg;
    double vias=30.0;
    double dif;
    if(abs(last_deg)<23){
    
        dif=(((Compass / 10) + 540 - CompassDef) % 360) - REFERENCE;
        if(abs(dif)<vias){
        
            pwm[0] = vr;
            pwm[1] = 0;
            pwm[2] = 0;
            pwm[3] = vl;
        }
        else{
            
            if(dif<0){
                //vs = vs - (20-vias);
                vs = -(20+(abs(dif))*0.1);
                if( abs(dif) >= 100) vs = -(20+(100)*0.1);
            }
            else{
                //vs = vs + (20-vias);
                vs = (20+(abs(dif))*0.1);
                if( abs(dif) >= 100) vs = (20+(100)*0.1);
            }
            
            pwm[0] = vs;
            pwm[1] = 0;
            pwm[2] = 0;
            pwm[3] = -vs;
        }    
    }else{        
        pwm[0] = vr;
        pwm[1] = 0;
        pwm[2] = 0;
        pwm[3] = vl;
    }
    
    
    if((!WaitFlag)&&(!(last_deg == Degree ))){
        SetDegree = Degree * 11/9;
        if(SetDegree > 110){
            SetDegree = 110;
        }else if(SetDegree < -110){
            SetDegree = -110;
        }
        S555.position(SetDegree);
        WaitFlag = 1;
        STimer.attach(&ServoWait,WAIT_TIME);
        
        last_deg = Degree;
    }   
    
    
    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];
    }
    

    wait_ms(10);
}

uint8_t PingChange(uint8_t LineData){
    static uint8_t Last_Line;
    static uint8_t Last_Ping;
    uint8_t LinePing = 0;
    
    if(!LineData){
        return 0;
    }    

    if(PingData[0] <50) LinePing = LinePing + 1;
    if(PingData[1] <50) LinePing = LinePing + 2;
    if(PingData[2] <50) LinePing = LinePing + 4;
    if(PingData[3] <50) LinePing = LinePing + 8;
    
    
    if(LinePing&0x01){
        if((LineData&0x01) ||(Last_Line&0x01)||(Last_Ping&0x01)){
            Last_Ping = LinePing;
            Last_Line = LineData;
            return 1;
        }
    }
    if(LinePing&0x02){
        if((LineData&0x02) ||(Last_Line&0x02)||(Last_Ping&0x02)){
            Last_Ping = LinePing;
            Last_Line = LineData;
            return 2;
        }
    }
    if(LinePing&0x04){
        if((LineData&0x04) ||(Last_Line&0x04)||(Last_Ping&0x04)){
            Last_Ping = LinePing;
            Last_Line = LineData;
            return 4;
        }
    }
    if(LinePing&0x08){
        if((LineData&0x08) ||(Last_Line&0x08)||(Last_Ping&0x08)){
            Last_Ping = LinePing;
            Last_Line = LineData;
            return 8;
        }
    }
    Last_Ping = 0;
    Last_Line = 0;
    return 0;
    
}

void fool (int *Degree, int *Power){
    static int Last_Degree = 0;
    static int Last_Vector = 1;
    int degree = *Degree;
    int Temp;
    
    
    if((*Degree <0)||(*Degree >=360)){
        *Degree = 0;
        Last_Degree = 0;
        Last_Vector = 1;
        return ;
    }
    
    Temp = Last_Degree % 180;
    if((Temp>70) &&(Temp<110)){
        Temp = *Degree % 180;
        if((Temp>70) &&(Temp<110)){
            Temp = abs(*Degree - Last_Degree);
            if(Temp>160){
                Last_Vector = -1 * Last_Vector;//正転逆転切り替え
                if(*Degree/180){
                    *Degree = Angle[Last_Degree%180] -(Last_Degree - *Degree%180);
                    *Power = *Power * Last_Vector;
                }else{
                    *Degree = Angle[Last_Degree%180] -(Last_Degree%180 - *Degree);
                    *Power = *Power * Last_Vector;
                }
                Last_Degree = degree;
                if((*Degree <= -120)||(*Degree >=120)){
                    *Degree = 0;
                }
                return;
            }else if((Last_Vector+2) == 1){
                /*逆転のまま角度拡張*/
                if(*Degree/180){
                    *Degree = -360 + *Degree ;
                    
                }
                *Power = *Power * Last_Vector;
                Last_Degree = degree;
                if((*Degree <= -120)||(*Degree >=120)){
                    *Degree = 0;
                    
                }
                return;

            }else if((Last_Vector+2) == 3){
            /*正転のまま*/
                if(*Degree/180){
                    *Degree = -360 + *Degree ;   
                }
                *Power = *Power * Last_Vector;
                Last_Degree = degree;
                if((*Degree <= -120)||(*Degree >=120)){
                    *Degree = 0;
                }
                return;
            }
        }
    }
    /*通常動作*/
    if(*Degree == 0){    
        Last_Vector = DegreeToVector[0];
        *Degree = Angle[*Degree%180];
        *Power = *Power * Last_Vector;
        Last_Degree = degree;
        return ;
    }
    if(*Degree == 270){
        Last_Vector = DegreeToVector[(*Degree)/90];
        *Degree = -90;
        *Power = *Power * Last_Vector;
        Last_Degree = degree;
        return;
    }    
    Last_Vector = DegreeToVector[(*Degree-1)/90];
    *Degree = Angle[*Degree%180];
    *Power = *Power * Last_Vector;
    Last_Degree = degree;
    if((*Degree <= -120)||(*Degree >=120)){
        *Degree = 0;
    }

}
int IrDegree(){
 /*irの1位の値，2位の場所からirの360へ持っていく*/
 
    uint8_t IrF ,IrS;
    unsigned int irdegree = 0;
    if(IrNum >=12){
        return 0;   
    }
    IrF = IrData[0]/12;
    IrS = IrData[0]%12;
    
    if(IrF == 0 ){
        if(IrS == 11){
            irdegree = 15;
                
        }else if(IrS == 1){
            irdegree = 345;    
        }
        return irdegree;
    }
    irdegree = 360 - IrF*30;    
    if(IrS&&(abs(IrF-IrS) == 1)){
        irdegree = irdegree - (IrF - IrS)*15;
            
    }
    if(irdegree>=360){
        return 0;
    }
        
    return irdegree;
}


void IrFrontAction()//ball 12 or 0 o-clock
{
    if(IrData[1]>70){
        move(30,30,CompassPID,0);
        return;
    }
    if(IrData[1]>60){
        move(20,20,CompassPID,0);
        return;
    }
    /*IrData[1]>500*/
    move(10,10,0,0);    
    if(PingData[0]<50){
        if((PingData[1]<60)&&(PingData[1]>40)){
        /*右側に居る*/
            move(40,25,0,10);
            Kick = 1;
            wait_ms(200);
            move(30,30,0,0);
            Kick = 0;
            return;
        }
        if((PingData[1]<60)&&(PingData[1]>40)){
        /*左側に居る*/
            move(25,40,0,-10);
            Kick = 1;
            wait_ms(200);
            move(30,30,0,0);
            Kick = 0;
            return;
        }
        /*それ以外*/
        
        //Receive();
        if(!(IrNum == 0)) return;
        move(40,40,CompassPID,0);
        Kick = 1;
        wait_ms(100);
        Kick = 0;
        return ;
    }
    move(20,20,CompassPID,0);
    
    
    
}

void IrBackAction()//ball found six o-clock
{
    /***
     * 6時にボールがある場合の処理．右と左のスペースを確認して広い方から回り込む
     *
     **/
    if(PingData[1]>PingData[3]){
        /*右が大きい*/
        if(IrData[1]>70){
            move(-20,-20,CompassPID,45);
            return;
        }
        if(IrData[1]>60){
            move(-20,-20,CompassPID,60);
            return;
        }        
        move(-20,-20,CompassPID,80);
        return;
    }
    /*左が大きい*/
    
    if(IrData[1]>70){
        move(-20,-20,CompassPID,-45);
        return;
    }
    if(IrData[1]>60){
        move(-20,-20,CompassPID,-60);
        return;
    }    
    move(-20,-20,CompassPID,-80);
    return;

}
/*
void GoHome()//Ball is not found.
{
    int pingRl =0;
    int PowLev;
    if(PingData[2] >=50){//後ろからの距離60cm
        move(-25,-25,CompassPID,0);
        return ;
    }
    if(PingData[2] <= 30){
        move(17,17,0,0);
        return ;
    }
    pingRl = PingData[1]+PingData[3];
    if(pingRl <100){
        move(0,0,CompassPID,0);//stop
        return;
    }
    if(PingData[1] < 52){
        PowLev = (52 - PingData[1])*1.35;
        move(PowLev,PowLev,0,45);
        return ;
    }else if (PingData[3] < 52){
        PowLev = (52 - PingData[3])*1.35;
        move(PowLev,PowLev,0,-45);
        return ;
    }

    move(0,0,0,0);//stop

}*/
void GoHome()//Ball is not found.
{
    int pingRl =0;
    int PowLev;
    if(PingData[2] >=70){//後ろからの距離60cm
        move(-25,-25,0,0);
        return ;
    }
    
    
    pingRl = PingData[1]+PingData[3];
    if(pingRl <120){
        if(PingData[2]<30){
            if(PingData[1]<30){
                move(15,15,0,30);
            }else if(PingData[3]<30){
                move(15,15,0,-30);
            }
        }
        move(0,0,0,0);//stop
        return;
    }
    if(PingData[1] < 65){
        PowLev = (52 - PingData[1])*1.35;
        move(-PowLev,-PowLev,0,-20);
        return ;
    }else if (PingData[3] < 65){
        PowLev = (52 - PingData[3])*1.35;
        move(-PowLev,-PowLev,0,20);
        return ;
    }
    if(PingData[2]<40){
        move(0,0,0,0);//stop
    }else{
        move(-15,-15,0,0);//stop
    
    }
    
    
    
}



void PidUpdate()
{   

    pid.setSetPoint((int)((REFERENCE + SetC) / 1.0)); 
    InputPID = ((Compass/10) + 540 - CompassDef) % 360;
 
    pid.setProcessValue(InputPID);
    CompassPID = (pid.compute());
    /*
    pid.setSetPoint((int)((REFERENCE + SetC) / 1.0)); 
    InputPID = Compass;

    pid.setProcessValue(InputPID);
    CompassPID = (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 == Kicker
       ||temp == Debug1
       ||temp == Debug2
       ||temp == Debug3
       ||temp == Debug12
       ||temp == Debug23
       ||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());
}

void SetUp(){
    /*初期化*/

    Motor.baud(115200);                             //ボーレート設定
    Motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
    Motor.attach(&tx_motor,Serial::TxIrq);          //送信空き割り込み（モータ用）
    
    Mbed.attach(&micon_rx,Serial::RxIrq);         //送信空き割り込み（センサ用）
    
    S555.calibrate(0.0006, 120.0);
    //S555.position(0.0);    //初期位置にセット
    move(0,0,0,0);//停止
    
    
    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  

    
    for(int i=0; i<15; i++){
        CompassDef = Compass/10;
        wait_ms(10);
    }
    //pidupdate.attach(&PidUpdate, PID_CYCLE);
    
}
void StartLoop(){

     uint8_t State = 0;
     uint8_t LineData = 0;
     int a;
     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)){
                LineData = (~Line+0x00) & 0x0F;
                Lcd.printf("%d\n",LineData);
                
                wait_ms(100); 
                State = SwRead();
            }   
            Lcd.cls(); 
            continue;
            
        }
        if(State == Debug2){
             while((State == Debug2)){
                //Receive();
                Lcd.printf("%d\n",IrNum);
                wait_ms(100);
                State = SwRead();
             }
             Lcd.cls();
             continue;
        }

        if(State == Debug3){
            while((State == Debug3)){
                a = (Compass/10 + 540 - CompassDef) % 360;
                Lcd.printf("%d\n",a);
                wait_ms(100);
                State = SwRead(); 
            }   
            Lcd.cls(); 
            continue;
        }
        if(State == Debug12){
            while((State == Debug12)){
                Lcd.printf("%d %d\n%d %d\n",PingData[0],PingData[1],PingData[2],PingData[3]);
                wait_ms(100);
                State = SwRead(); 
            }   
            Lcd.cls(); 
            continue;
        }
        if(State == Kicker){
            Led[0] = Led[1] = Led[2] = 0;
            Kick = 1;
            wait_ms(500);
            Kick = 0; 
            while((State == Kicker)){
                wait_ms(100); 
                State = SwRead();
            }   
            continue;   
        }
    }     
    
}
int main() {
    
    //uint8_t IrNumOld = 0;//過去値
    /*Line*/
    uint8_t LineData = 0;
    uint8_t LinePing = 0;


    /*State */ 
    uint8_t LineIr = 0;
    uint8_t IrChange[13] ={0x01,0x01,0x02,0x02,0x02,0x04,
                           
                           0x04,0x04,0x08,0x08,0x08,0x01,0x00};

    /*行動設定*/
    int Power = 0;
    int Degree = 0;
    
    /*楽しい変数達*/
    int nDegree =0;//基礎角
    int addDegree = 0;//追加角
    int Temp = 0;
    
    int  StopCt = 0;
    
    /*関数ポインタ*/


    //void (*AnotherAction[3])(uint8_t [],double);
    void (*AnotherAction[3])();
    AnotherAction[0] = IrFrontAction;
    AnotherAction[1] = IrBackAction;
    AnotherAction[2] = GoHome;
    

    SetUp();/*set up routine*/

    StartLoop(); /*loop strat, switch push end*/
    Led[0] = Led[1] = Led[2] = Led[3] = 0;
    wait_ms(100);
    
    while(1){
         
        //S555.calibrate(0.0006, 120.0);

        //Receive();
        //Lcd.printf("%d\n",IrNum);
        /*白線を読んでいないか確認する*/
        
        LineData = (~Line+0x00) & 0x0F; 
        
        if(LineData){
            LineIr = LineData & IrChange[IrNum];
            LinePing = PingChange(LineData);
            if(LinePing){
                move(0,0,0,0);
                wait_ms(200);
            }
            if(LineIr){
                move(0,0,0,0);
                StopCt = 0;
                while(LineIr){
                    if(StopCt>30){
                        GoHome();
                        wait_ms(100);
                    }
                    StopCt++;
                    Led[1] = Led[2] = Led[3] = 1;  
                    //Receive();
                    LineData = (~Line+0x00) & 0x0F;
                    LineIr = LineData & IrChange[IrNum];
                    wait_ms(10);
                }                 
            }else if(LinePing){
                move(0,0,0,0);
                StopCt = 0;
                while(LinePing){
                    if(StopCt>10){
                        GoHome();
                        wait_ms(200);
                    }
                    StopCt++;
                    
                    Led[1] = Led[2] = Led[3] = 1;  
                    //Receive();
                    LineData = (~Line+0x00) & 0x0F;
                    LinePing = PingChange(LineData);
                    
                    wait_ms(10);
                }
            }
            
            
            Led[1] = Led[2] = Led[3] = 0;
            
        }
        
        Power = 0;
        Led[0] = 1;
        Degree = 0;
        SetC = 0.0;
        
        
        Led[3] = 1;
        
        Degree = IrDegree();
        
        if((Degree == 0)||(Degree == 180)||(IrNum == 12)){
            (AnotherAction[IrNum/6])();
            continue;
        }
        
        /*
        if(IrNum == 12){
            move(0,0,0,0);
            wait_ms(10);
            continue;   
        }*/
        
        nDegree = wrapDegree[Degree/15];
        Power = 30;
        Temp = Degree%180;
        
        if((Temp>70)&&(Temp<110)){
            if(PingData[2]<40){
                if(IrData[1] > 70){
                    if(Degree/180){
                        addDegree = 90;     
                    }else{
                        addDegree = -90;
                    }   
                }else if(IrData[1] >60){
                    if(Degree/180){
                        addDegree = 60;     
                    }else{
                        addDegree = -60;
                    }
                }else{  
                    addDegree = 0;
                }
            }else{
                if(IrData[1] > 70){
                    if(Degree/180){
                    addDegree = 30;    
                    
                    }else{
                    
                    addDegree = -30;
                    }
                    
                }else if(IrData[1] >60){
                    if(Degree/180){
                    addDegree = 20;    
                    
                    }else{
                    addDegree = -20;
                   }
                  
                
                }else {
                    addDegree = 0;
                    
                } 
                    
            }
        }else{   
            if(IrData[1] > 70){
                if(Degree/180){
                addDegree = 30;    
                
                }else{
                
                addDegree = -30;
                }
                
            }else if(IrData[1] >60){
                if(Degree/180){
                addDegree = 20;    
                
                }else{
                addDegree = -20;
               }
              
            
            }else {
                addDegree = 0;
                
            }
        }    
        if(PingData[2]<40){
            if((Degree>150)&&(Degree<300))
                if(Degree/180){
                    addDegree = 60;    
                
                }else{
                
                    addDegree = -60;
            }
        }
                
        Degree = nDegree + addDegree;
        
        if((Degree <0)||(Degree>=360)){
            Degree = 0;
        }
        fool(&Degree,&Power);
        move(Power,Power,CompassPID,Degree);
        
        //wait_ms(500);
        Led[0] =0;
        wait_ms(10);
        
    }
    int i;
    while(1){
    //デモプログラム
        //Receive();
        for(i = 0; i<360; i++){
            Power = 20;
            fool(&i,&Power);
            
            move(Power,Power,0,i);
            wait_ms(200);
        }
        //pc.printf("%d %d %d %d %d\n",IrData[0],IrData[1],IrData[2],PingData[0],PingData[1]);
        //pc.printf("%d %d %d %d\n",PingData[1],PingData[2],PingData[3],Compass);
        
        //pc.printf("%d\t %d\t %d\t %d\t %d\t %d\t\n",rx_data[3],rx_data[4],rx_data[5],rx_data[6],rx_data[7],rx_data[8]);
        //pc.printf("%d\t %d\t %d\t %d\n",speed[0],speed[1],speed[2],speed[3]);
        wait(0.1);
    }
    
    
    
    
}