File content as of revision 2:054444aa1990:

/***********************************
 *RoboCupJunior Soccer B Open 2014 
 *Koshinestu progrum
 *
 *このプログラムでは以下の処理をする．
 *　LPC1114FN28/102からI2Cを用いて各種センサデータを収集
 *　
 *　データからロボットの移動やキッカー等のモータの動作を決定する処理を行う
 *
 *　MotorDriverにmaxonに命令
 *　
 *　SteppingMotorDriverにステアリング命令
 *　
 *　LCDでデバック
 *
 *　スイッチ4つとスタートスイッチで処理を実行
 *  
 * キッカーのスイッチがどこかに入る
 *
 ************************* 
 * 
 *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 <math.h>
#include <sstream>

#define ADDRESS_R 0xA0
#define ADDRESS_L 0xC0

#define READ_IR 0x01

DigitalIn   DebugSw[4] = {p22,p23,p24,p25};
DigitalIn   StartSw(p26,PullUp);
DigitalOut  Led[4] = {LED1,LED2,LED3,LED4};

I2C         Sensor(p9,p10);//sda,scl
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);


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


int speed[4] = {0};            


unsigned int IrReceive(uint8_t *far){
    /*
    Slave側はIRを要求した場合IRのみを返してくるとする．
    irは値が大きいほうが近いと仮定する
    
    
    */
    char data_r[4],data_l[4];
    bool val;
    Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか
    
    val = Sensor.read(ADDRESS_R|1, data_r, 4);// PINGデータを受信
    Led[0] = val;
    wait_ms(5);
    val = Sensor.read(ADDRESS_L|1, data_l, 4);
    Led[0] = !val;
    
    if((data_r[3]-data_l[3])>0)
        *far = data_r[0]%36%6*6 + (data_l[0]%36%6+6);
        
    if(data_r[2]>data_l[2]){
        if(data_r[2]>data_l[1]){   
            return data_r[0]/36*12*12*12 + data_r[0]%36/6*12*12 + (data_l[0]/36+6)*12 + (data_l[0]%36/6+6);
        }else{
            return data_r[0]/36*12*12*12 +(data_l[0]/36+6)*12*12+ data_r[0]%36/6*12 +(data_l[0]%36/6+6);
        }
    }else{
        if(data_l[2]>data_r[1]){   
            return (data_l[0]/36+6)*12*12*12 + (data_l[0]%36/6+6)*12*12 + data_r[0]/36*12 + data_r[0]%36/6;
        }else{
            return (data_l[0]/36+6)*12*12*12 +data_r[0]/36*12*12+ (data_l[0]%36/6+6)*12 +(data_r[0]%36/6+6);
        }
        
    }
    
}

unsigned int LineReceive(){
    //先に要求しない場合はLineの状況を送信すること．
    //順番適当
    char data_r[2],data_l[2];
    bool val;
    //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[1]<<8 + data_l[1];
    

}
void rotate(unsigned int times, bool dir){
    /*
     *回転速度，回転方向，回転角度等設定変更ののち回転．
     *
     *
     *
     */
    
    
}

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 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);          //送信空き割り込み（モータ用）
    move(0,0);//停止

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

    Lcd.cls();
    
    /*初期化終了*/
    
    
}
void StartLoop(){
    /*
     *スイッチが押されるまでロボットはスタートしない．
     *
     *待っている間にほかのスイッチが押された場合は押されている間その動作をする等.
     *
     *
     */
    
}
int main() {

    /*IrRanking*/
    unsigned int IrNigh;//ranking min1  > min2 > min3 > min4 
    uint8_t IrFar;//far1 > far2;
    
    /*Line*/
    unsigned int Line;
    
    /*Ping(仮)*/
    
    
    /*move value*/
     int  vr,vl;
    
    SetUp();
    
    StartLoop();
    
    while(1) {
        
        IrNigh = IrReceive(&IrFar);//順位設定
        
        /*ここにIRの場所によったプログラムを挿入*/
        
        /*line*/
        Line = LineReceive();
        
                

        move(30,30);
        wait(1.5);

    }
}