File content as of revision 6:c2c31bc971ad:

/***********************************
 *RoboCupJunior Soccer B Open 2014 
 *Koshinestu progrum
 *
 *このプログラムでは以下の処理をする．
 *　LPC1114FN28/102からI2Cを用いて各種センサデータを収集
 *　
 *　データからロボットの移動やキッカー等のモータの動作を決定する処理を行う
 *
 *　MotorDriverにmaxonに命令
 *　
 *　SteppingMotorDriverにステアリング命令
 *　
 *　LCDでデバック
 *
 *　スイッチ4つとスタートスイッチで処理を実行
 *  
 * キッカー用のDigitalOutがどこかに入る
 *
 ************************* 
 * 
 *Pin Map
 *
 *  p5,p6,p7,p29,p30 >> SPI >>Stepping
 *  
 *  p9,p10 >> I2C orSerial >> LPC1114FN28/102 read
 *
 *  p13,p14 >> Serial >> Motor
 *
 *  p22~p26 >> DebugSw and StartSw 
 *
 *  p27,p28 >> I2C  >> DebugLCD
 *
 *
 *
 ******************************/


/*

****以下IRはNighは距離が近いものとする．

*/






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

/*回り込みの計算用*/
#define PI 3.141593/*割と早めにきってある*/
#define SHORT_LEN 15 /*cm換算　楕円のB辺の長さを定義しておく*/

#define ADDRESS_R 0xA0
#define ADDRESS_L 0xC0

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

#define READ_IR    0x01 //送る物指定
#define READ_PING  0x02




BusIn   Sw(p22,p23,p24,p25,p26);
DigitalOut  Led[4] = {LED1,LED2,LED3,LED4};

I2C         Sensor(p9,p10);//sda,scl
HMC6352     hmc6352(p9,p10);
AQM0802A    Lcd(p28,p27);//sda,scl
L6470       Step(p5,p6,p7,p29,p30);//mosi,miso,sck,#cs,busy(PullUp)
Serial      Motor(p13,p14);//tx,rx
Serial      pc(USBTX,USBRX);

DigitalOut  Kick(p21);

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

int speed[4] = {0};            

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


unsigned int IrReceiveR(){
    /*順位解析版
    Slave側はIRを要求した場合IRのみを返してくるとする．
    irは値が大きいほうが近いと仮定する
    
    
    */
    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)){
            return 12;
        }
        if(data_r[0] == 0){
            return (data_l[0]/6+6)*12*12 + (data_l[0]%6+6)*12 + data_r[0]/6;
        }
        return data_r[0]/6*12*12 + (data_r[0]%6)*12 + data_l[0]/6+6;
       
    }
    
    if(data_r[2]>data_l[2]){
        if(data_r[2]>data_l[1]){   
            return data_r[0]/6*12*12 + (data_r[0]%6)*12 + data_l[0]/6+6;
        }
        
        return data_r[0]/6*12*12 +(data_l[0]/6+6)*12+ data_r[0]%6;
        
    }else{
        if(data_l[2]>data_r[1]){   
            return (data_l[0]/6+6)*12*12 + (data_l[0]%6+6)*12 + data_r[0]/6;
        }
        
        return (data_l[0]/6+6)*12*12 +(data_r[0]/6)*12+ (data_l[0]%6+6);      
    }
    
}

uint8_t IrReceiveS(){
    /*単純解析版 一番大きい位置だけ返す
    
    Slave側はIRを要求した場合IRのみを返してくるとする．
    irは値が大きいほうが近いと仮定する
    
    */
    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)){
            return 12;
        }
        if(data_r[0] == 0){
            return data_l[0]/6+6;
        }
        return data_r[0]/6;  
    }

    if(data_r[2]>data_l[2]){   
        return data_r[0]/6;
    }
    return data_l[0]/6+6;
    
}
uint8_t IrReceiveM(){
    /*値解析版　一番近い場所の値を返す（位置はとりあえずない）
    Slave側はIRを要求した場合IRのみを返してくるとする．
    irは値が大きいほうが近いと仮定する
    
    
    */
    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)){
            return 255;
        }
        if(data_r[0] == 0){
            return data_l[1];
        }
        return data_r[1];   
    }
    
    
    if(data_r[2]>data_l[2]){       
        return data_r[1];
        
    }else{ 
        return data_l[1];
        
    }
    
}
unsigned int IrReceiveSM(){
    /*値解析版　一番大きい値とその位置を返す
    Slave側はIRを要求した場合IRのみを返してくるとする．
    irは値が大きいほうが近いと仮定する
    
    
    */
    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)){
            return 12<<8+255;
        }
        if(data_r[0] == 0){
            return (data_l[0]/6+6)<<8 + data_l[1];
        }
        return data_r[0]/6<<8 + data_r[1];
    }
    
    
    if(data_r[2]>data_l[2]){       
        return data_r[0]/6<<8 + data_r[1];
        
    }
    return ((data_l[0]/6)+6) <<8 + data_l[1];
    
    
}
uint8_t  LineReceive(){
    //先に要求しない場合はLineの状況を送信すること．//4bit //前ー右ー後ろー左　
    char data_r[2],data_l[2];
    bool val;
    //example val = slave.read(address,data,length,repeat);
    val = Sensor.read(ADDRESS_R|1, data_r, 1);
    Led[1] = val;
    wait_ms(5);
    val = Sensor.read(ADDRESS_L|1, data_l, 1);
    Led[1] = !val;
    
    
    return data_r[0]<<2 + data_l[0];
    

}


void PingReceiveRL(char ping[]){//ping[0]==Right,ping[1]==Left
    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 rotate(unsigned int times, bool dir){
    /*
     *回転速度，回転方向，回転角度等設定変更ののち回転．
     * this is 使わなそうである、
     */
     
     
     
         
    
}

void move(int vr, int vl){
    double pwm[4] = {0};
    uint8_t i = 0;
    pwm[0] = vr/10.0;
    pwm[1] = vl/10.0;
    pwm[2] = 0;
    pwm[3] = 0;

    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 ControlRobo0(int *CompassDef)//LeftFront
{}
void ControlRobo1(int *CompassDef)//Front
{
/*前にボールがある場合の動作*/
    int Compass;
    char Ping[2];
    uint8_t Line,LineIr;
    unsigned int IrNumMax; 
    
    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;
        }
        return;
    }
    if(IrReceiveM()<=150){//適当な150
        /*ステッピングを正面に設定*/
        /*busy_wait()*/
        /*モーターを前進*/
        return;
    }
    PingReceiveRL(Ping);
    if((Ping[0]>90)&&(Ping[1]>40)){//数値は適当
        /*ステッピングを少しだけ傾ける*/
        /*モーターを左右差つけて回す(ロボットは少し傾く)*/
        /*busy_wait()*/
        /*モーターを前進*/
        while(1){
            Line = LineReceive();
        
            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 ControlRobo2(int *CompassDef)//RightFront
{
    



}
void ControlRobo3(int *CompassDef)//RightFront
{}
void ControlRobo4(int *CompassDef)//Right
{
    int a = 50;
    unsigned int  ir_sm;
    int kakudo = -90;
    double value = 0;
    double v = 50;/* cm/s */
    double x = 20;/*モーターを回す割合,今は適当*/
    
    ir_sm = IrReceiveSM();
    if(ir_sm&0xF00>>8 == 4){
        return;
    } 
    a = 300 - ir_sm&0x00FF ;//適当な計算//300は勘
    value = 10 * a * SHORT_LEN + 3*(a * a + SHORT_LEN * SHORT_LEN);
    value = PI *(3 * ( a + SHORT_LEN) - sqrt(value))/4/v;
    
    if(300 - ir_sm&0x00FF >200){
        Step.Step(-10);//適当
        Step.BusyWait(0);   
    }
    
    /*SetPerm()
     *valueに応じた何かを計算する
     *ACCまたはMAX_SPEEDをいじればいい気がする
     *
     */
    Step.Step(kakudo+10);
    
    /*x = v*何か*/
    move(-x,-x);
    

}
void ControlRobo5(int *CompassDef)//RightBack
{}
void ControlRobo6(int *CompassDef)//BackRight
{}
void ControlRobo7(int *CompassDef)//Back
{
    /*****
     * this function is drawing a semicircle. 
     * semicircle manipulated ir_data&ping_data.
     * 
     * 変数名は後から変えるためにわかりやすい名前にしておく
     **/
    uint8_t ir_num;
    uint8_t pingl,pingr; 
    
    /*
    ir(1位,2位,必要なら距離も)と超音波のデータを取得
    */
    if(ir_num==7){//一致しているかどうか念のため
        return ;
    }
    if(pingr<pingl){//本当ならば壁と垂直かどうか、確認すべき。コンパスや超音波の合計を見れば可能
        /*軌道の計算、楕円の半分見たいな感じの軌道がベスト*/
        /*半円なのであらかじめステッピングを回転させる必要がある*/
        /*モーター設定*/
        /*ステッピング設定*/
        return;
    }
    /*軌道の計算、楕円の半分見たいな感じの軌道がベスト*/
    /*半円なのであらかじめステッピングを回転させる必要がある*/
    /*モーター設定*/
    /*ステッピング設定*/
    return;
    
    
    }
void ControlRobo8(int *CompassDef)//BackLeft
{}
void ControlRobo9(int *CompassDef)//LeftBack
{}
void ControlRobo10(int *CompassDef)//Left
{}
void ControlRobo11(int *CompassDef)//LeftFront
{}
void GoHome(int *CompassDef)//Ball is not found.
{
        
    
    

}


uint8_t SwRead(){
/******
 *返却値はスイッチの状況
 *参照元::aaaaa
 *
 *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);//停止

    Step.Resets();
    Step.ReleseSW(0,1);//記憶がない

    hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    *compass_def = (hmc6352.sample() / 10);
    
    Lcd.cls();
    
    /*初期化終了*/
    
    
}
void StartLoop(){
    /*
     *スイッチが押されるまでロボットはスタートしない．
     *
     *待っている間にほかのスイッチが押された場合は押されている間その動作をする等.
     *
     *switch割り当て
     *1.コンパスのキャリブレーション実行スイッチ
     *2.キッカーのキック
     *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はボールがない状態
    
    /*Line*/
    uint8_t Line;
    
    /*Compass*/
    int CompassDef = 0, Compass = 0;
    
    /*State */ 
    //Direction LineIr = NA;//方位設定奴...エラーでてめんどくさいので後でまたやることにする。
    uint8_t LineIr = NA;
    
    
    
    /*関数ポインタ*/
    void (*ControlRobo[13])(int *);//irの位置による分岐
    ControlRobo[0] = ControlRobo0;
    ControlRobo[1] = ControlRobo1;
    ControlRobo[2] = ControlRobo2;
    ControlRobo[3] = ControlRobo3;
    ControlRobo[4] = ControlRobo4;
    ControlRobo[5] = ControlRobo5;
    ControlRobo[6] = ControlRobo6;
    ControlRobo[7] = ControlRobo7;
    ControlRobo[8] = ControlRobo8;
    ControlRobo[9] = ControlRobo9;
    ControlRobo[10] = ControlRobo10;
    ControlRobo[11] = ControlRobo11;
    ControlRobo[12] = GoHome;
    
    
    SetUp(&CompassDef);/*set up routine*/
    
    StartLoop(); /*loop stat, switch push end*/
    
    /*
    前を向くように設定をするべき
    アウトオブバウンズからの復帰時に反対を向けてスタートなので必要。
    */
    while(1){//前を向かせるwhile
        Compass = ((hmc6352.sample() / 10) + 540 - CompassDef) % 360;
        if(Compass == 3) break;//前を向いたら抜ける。
    }
    while(1) {
        
        Line = LineReceive();
        
        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;
            continue;
        
        }
        
        if(Step.BusyCheck()){//ステッピングが動いていないか
            continue;
            
        }
        
        IrNum = IrReceiveS();
        
        (*ControlRobo[IrNum])(&CompassDef);//irの位置によったループ等の処理に移る。
        
        //wait_ms(0);
        
    }
}