File content as of revision 0:fb4c529248d7:

#include "mbed.h"
//#include "HMC6352.h"
#include "TextLCD.h"
#include "common.h"
#include "PID.h"
#include <math.h>
#include <sstream>

#define LINE_LP 30/*line def@line_state*/
#define LINE_FP 40/*line def@line_State*/
#define LINE_ON 0x2710/*line underline*///40000
#define LINE_TIME 0.5/*used lineAttach*/

/****pid config *************/
#define RATE    0.08//52
#define PID_BIAS    0.2
#define REFERENCE   180.0
#define MINIMUM     0.1
#define MAXIMUM     360.0
#define PID_CYCLE   0.05   //s
#define P_GAIN  1.4    //0.78   
#define I_GAIN  0.0     //0.0
#define D_GAIN  0.015  //0.009
#define OUT_LIMIT   30.0
#define MAX_POW     100
#define MIN_POW     -100
/****pid config *************/

 /*keep your distance!!*/
#define OFFENSE_POWER 31  /*moter power strength*/
#define DEFENSE_POWER 30

#define BACK_HOME_TIME 0.3

/*dash*/
#define DASH_TIME 0.7//about
#define CHARGE_DACH_TIME 0.25//about
#define DASH_STRENGTH 40

Serial motor(p9,p10);
Serial sensor(p13,p14);
Serial xbee(p28,p27);
Serial pc(USBTX, USBRX);

TextLCD lcd(p26, p25, p24, p23, p22, p21);
AnalogIn adcline[4] = {p17, p18, p19, p20};
DigitalIn start(p7);/*start switch*/
DigitalIn check(p8);/*Xbee chenge switch*/
DigitalOut myled[4] = {LED1, LED2, LED3, LED4};
DigitalIn kick_check(p5);

/*Compass PID SetUp*/
PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);
Ticker pidUpdata;

/*XBee interrupt*/
Ticker xbeetx;
Ticker xbeerx;

/*line check,linestate*/
Timeout liner0;
Timeout liner1;
Timeout liner2;
Timeout liner3;

/*back home*/
Timeout home;
Timer timer_home;

/*ofence dash timer*/
Timer dash_charge;
Ticker dash_start;


extern string StringFIN;
extern void array(int,int,int,int);
extern void micon_rx(void);
extern void xbee_tx(void);
extern void xbee_rx(void);
extern int count;

int speed[4] = {0};                                        /*@move,tx_moter*/
uint8_t line_stop[4] = {0},home_stop = 0;                  /*@line_stop(number),@back_home*/
unsigned int compass = 0;                                  /*relevant compass*/
double standTu = 0, inputPID = 180.0, compassPID = 0.0;    /*relevant compass*/
uint8_t ping[4]={0};                                       /*ping_data*/
uint8_t ir_min = 0,ir_num = 0, ir_main = 0;                /*ir_min=25~50~70(value),ir_num=0~9,ir_main=undef*/
//double clockL = 0;//not used
uint8_t dash_stop = 0;


void move(double vx, double vy, int vs, int vk){
    double pwm[4] = {0};
    uint8_t i = 0;
    pwm[0] = (double)((vx) + vs);
    pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0)  * vy) + vs);
    pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs);
    pwm[3] = vk;

    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 line_state(double *vx,double*vy,uint8_t *line){
    /*chenge to move ,line*/
    if(line[FRONT]){
        *vy = -LINE_FP;
    }
    if(line[LEFT]){
        *vx = LINE_LP;
    }
    if(line[BACK]){
        *vy = LINE_FP;
    }
    if(line[RIGHT]){
        *vx = -LINE_LP;
    }
}
void line_stop0(){
    line_stop[FRONT] = 0;
}
void line_stop1(){
    line_stop[LEFT] = 0;
}
void line_stop2(){
    line_stop[BACK] = 0;
}
void line_stop3(){
    line_stop[RIGHT] = 0;
}
void line_check(uint8_t *line){
    if(!line_stop[FRONT]){
        if(adcline[FRONT].read_u16() > LINE_ON){
            line[FRONT] = 1;
            line_stop[FRONT] = 1;
            line_stop[BACK] = 1;
            myled[0] = 1;
            liner0.attach(&line_stop0,LINE_TIME);
            liner2.attach(&line_stop2,LINE_TIME);
        } else {
            line[FRONT] = 0;
            myled[0] = 0;
        }
    }
    if(!line_stop[LEFT]){
        if(adcline[LEFT].read_u16() > LINE_ON){
            line[LEFT] = 1;
            line_stop[LEFT] = 1;
            line_stop[RIGHT] = 1;
            myled[1] = 1;
            liner1.attach(&line_stop1,LINE_TIME);
            liner3.attach(&line_stop3,LINE_TIME);
        } else {
            line[LEFT] = 0;
            myled[1] = 0;
        }
    }
    if(!line_stop[BACK]){
        if(adcline[BACK].read_u16() > LINE_ON){
            line[BACK] = 1;
            line_stop[BACK] = 1;
            line_stop[FRONT] = 1;
            myled[2] = 1;
            liner2.attach(&line_stop2,LINE_TIME);
            liner0.attach(&line_stop0,LINE_TIME);
        } else {
            line[BACK] = 0;
            myled[2] = 0;
        }
    }
    if(!line_stop[RIGHT]){
        if(adcline[RIGHT].read_u16() > LINE_ON){
            line[RIGHT] = 1;
            line_stop[RIGHT] = 1;
            line_stop[LEFT] = 1;
            myled[3] = 1;
            liner3.attach(&line_stop3,LINE_TIME);
            liner1.attach(&line_stop1,LINE_TIME);
        } else {
            line[RIGHT] = 0;
            myled[3] = 0;
        }
    }
}
void PidUpdate()
{ 
    pid.setSetPoint((int)((REFERENCE + standTu) / 1.0)); 
    inputPID = compass;

    pid.setProcessValue(inputPID);
    compassPID = -(pid.compute());
}


void ir_defense0(double *vx, double *vy, int *k, int rateD){
    

}

void ir_defense1(double *vx, double *vy, int *k, int rateD){
       

}

void ir_defense2(double *vx, double *vy, int *k, int rateD){
       

}

void ir_defense3(double *vx, double *vy, int *k, int rateD){
       

}

void ir_defense4(double *vx, double *vy, int *k, int rateD){
       

}

void ir_defense5(double *vx, double *vy, int *k, int rateD){
       

}

void ir_defense6(double *vx, double *vy, int *k, int rateD){
       

}

void ir_defense7(double *vx, double *vy, int *k, int rateD){
    if(ping[2]<20){
        *vx = DEFENSE_POWER*1.2*way16[13][0];
        *vy = DEFENSE_POWER*1.2*way16[13][1];
    }
}

void ir_defense8(double *vx, double *vy, int *k, int rateD){
    if(ping[LEFT]>ping[RIGHT]){
        *vx = DEFENSE_POWER*(-0.8+near1[ir_num][VX]+go[ir_num][VX]);
        *vy = DEFENSE_POWER*(0.5+near1[ir_num][VY]+go[ir_num][VY]);
    }else if(ping[RIGHT]>ping[LEFT]){
        *vx = DEFENSE_POWER*(0.8+near1[ir_num][VX]+go[ir_num][VX]);
        *vy = DEFENSE_POWER*(0.5+near1[ir_num][VY]+go[ir_num][VY]);
    }

}

void ir_defense9(double *vx, double *vy, int *k, int rateD){
    if(ping[2]<20){
        *vx = DEFENSE_POWER*1.2*way16[3][0];
        *vy = DEFENSE_POWER*1.2*way16[3][1];
    }      

}

void ir_offense0(double *vx, double *vy, int *vk, int rateD){
    dash_charge.stop();
    dash_charge.reset();
    /*
    clockL = 6.50;
    standTu = 0;
    *vx = (ir_min+5)*way16[9][0];
    *vy = (ir_min+5)*way16[9][1];
    */
    
}
void ir_offense1(double *vx, double *vy, int *vk, int rateD){
    dash_charge.stop();
    dash_charge.reset();
    /*
    clockL = 7.00;
    standTu = 0;
    *vx = ir_min*way16[10][0];
    *vy = ir_min*way16[10][1];
    */
}
void ir_offense2(double *vx, double *vy, int *vk, int rateD){
    dash_charge.stop();
    dash_charge.reset();

    /*y
    if(rateD <= 0){

        *vx = STRENGTH*1.2*way16[0][0];
        *vy = STRENGTH*1.2*way16[0][1];
    }
    */
    /*
    clockL = 10.0;
    *vx=-ir_min;
    *vy = ir_min+5;
    if(ping[LEFT]>ping[RIGHT]){
        clockL = 10.5;
        *vx = 35*way16[14][0];
        *vy = 35*way16[14][1];
            *//**vx = 0;
            *vy =35;*/   
    /*
    }
    */
}
void ir_offense3(double *vx, double *vy, int *vk, int rateD){
    /**********
    一定時間待ち,その間変わらずir[3]が反応していたならば、ブースト直進キック
    irの最小値の位置が移動した場合,タイマーの時間を初期化、停止
    設定時間内に変更がなかった場合,数秒間キック直進し続ける。(ほかからの変更を拒否)
    
    ********/
    if(dash_charge.read() == 0){
        
        dash_charge.start();
    }
    /*
    if(rateD <=0+3){
        *vk = 25;
    }else {
        *vk = 0;
    }
    */
    /*
    if(rateD <= 0){
        *vx = STRENGTH*1.2*way16[0][0];
        *vy = STRENGTH*1.2*way16[0][1];
    }*/
    /*
    clockL = 12.0;
    *vx = 0,*vy = 30;
    */
}
void ir_offense4(double *vx, double *vy, int *vk, int rateD){
    dash_charge.stop();
    dash_charge.reset();

    /*if(rateD <= 0){

        *vx = STRENGTH*1.2*way16[0][0];
        *vy = STRENGTH*1.2*way16[0][1];
    }*/
    /*
    clockL = 2.00;
    *vx=ir_min;
    *vy = ir_min+5;
    if(ping[RIGHT]>ping[LEFT]){
        clockL = 2.50;
        *vx = 35*way16[2][0];
        *vy = 35*way16[2][1];
            *//**vx = 0;
            *vy = 35;*/
    /*
    }
    */
}
void ir_offense5(double *vx, double *vy, int *vk, int rateD){
    dash_charge.stop();
    dash_charge.reset();
    /*
    clockL = 5.00;
    standTu = 0;
    *vx = ir_min*way16[6][0];
    *vy = ir_min*way16[6][1];
    */
}
void ir_offense6(double *vx, double *vy, int *vk, int rateD){
    dash_charge.stop();
    dash_charge.reset();
    /*
    clockL = 5.50;
    standTu = 0;
    *vx = (ir_min+5)*way16[7][0];
    *vy = (ir_min+5)*way16[7][1];
    */
}
void ir_offense7(double *vx, double *vy, int *vk, int rateD){
    dash_charge.stop();
    dash_charge.reset();
    /*clockL = 6.00;
    standTu = 0;
    *vx = 0;
    *vy = -(ir_min+8);
    */
}
void ir_offense8(double *vx, double *vy, int *vk, int rateD){
    dash_charge.stop();
    dash_charge.reset();
    if(rateD<(0)){
        if(ping[LEFT]>ping[RIGHT]){
            *vx = OFFENSE_POWER*(-1+near1[ir_num][VX]+go[ir_num][VX]);
            *vy = OFFENSE_POWER*(1+near1[ir_num][VY]+go[ir_num][VY]);
        }else if(ping[RIGHT]>ping[LEFT]){
            *vx = OFFENSE_POWER*(1+near1[ir_num][VX]+go[ir_num][VX]);
            *vy = OFFENSE_POWER*(1+near1[ir_num][VY]+go[ir_num][VY]);
        }
    }
    /*standTu = 0;
    if(ping[LEFT]>ping[RIGHT]){
            clockL = 8.00;
            *vx = 40*way16[11][0];
            *vy = 40*way16[11][1];
        }else{
            clockL = 4.00;
            *vx = 40*way16[5][0];
            *vy = 40*way16[5][1];
        }*/
}
void ir_offense9(double *vx, double *vy, int *vk, int rateD){
    dash_charge.stop();
    dash_charge.reset();
    /*clockL = 6.00;
    standTu = 0;
    *vx = 0;
    *vy = -(ir_min+8);*/
}


/**kick and dash function**/
void dash_reset(){
    dash_stop = 0;
    lcd.cls();
}

void dist_fun_DM(int *rate_dm){//used defense first junction
    *rate_dm = ir_min - keep_ball[ir_num];//zastu
    if(*rate_dm >0){
        *rate_dm = 1;
    }else if(*rate_dm <-7){
        *rate_dm =-7;
    }

}

void dist_fun_D(int *rate_d){//ofense action support
    *rate_d = ir_main - keep_dist[ir_num];
    if(*rate_d < -5){
        *rate_d = -5;
    }else if(*rate_d >10){
        *rate_d = 11;
    }
}
void dist_fun_ping(int *rate_p){//defense action support
    rate_p[1] = ping[1] - keep_ping[1];
    if(rate_p[1] <-10){
        rate_p[1] = -10;
    }else if(rate_p[1]>0){
        rate_p[1] = 1;
    }
    rate_p[2] = keep_ping[2] - ping[2];
    if(rate_p[2]<-5){
        rate_p[2] = -5;
    }else if(rate_p[2] >6){
        rate_p[2] = 6;
    }
    rate_p[3] = ping[3] - keep_ping[3];
    if(rate_p[3] <-10){
        rate_p[3] = -10;
    }else if(rate_p[3]>0){
        rate_p[3] = 1;
    }
}

void dist_fun_pingSTRONG(int *rate_p){//defense action support
    rate_p[2] = keep_pingSTRONG[2] - ping[2];
    if(rate_p[2]<-5){
        rate_p[2] = -5;
    }else if(rate_p[2] >6){
        rate_p[2] = 6;
    }
    if(ping[1]+ping[3] >=155){//tyousei
        rate_p[1] = ping[1] - keep_pingSTRONG[1];//38
        if(rate_p[1] <-10){
            rate_p[1] = -10;
        }else if(rate_p[1]>0){
            rate_p[1] = 1;
        }
        
        rate_p[3] = ping[3] - keep_pingSTRONG[3];///37
        if(rate_p[3] <-10){
            rate_p[3] = -10;
        }else if(rate_p[3]>0){
            rate_p[3] = 1;
        }
        
    }else {
        rate_p[1] = 0;
        rate_p[3] = 0;
    }
}
/*
void dist_fun(int *rate_d,int *rate_dm,int *rate_p){*//*This is compute some length.?*/
    /* KEEP_DISTANCE is border line, ir_main is variable 
     if ir_main> keep_dist   (access)
     else ir_main< keep_dist (out)
    @retaD
    */
    /*
    *rate_d = ir_main - keep_dist[ir_num];
    if(*rate_d < -5){
        *rate_d = -5;
    }else if(*rate_d >10){
        *rate_d = 11; //+30  
    }
    *rate_dm = ir_min - keep_ball[ir_num];//zastu
    rate_p[1] = ping[1] - keep_ping[1];
    if(rate_p[1] <-10){
        rate_p[1] = -10;
    }else if(rate_p[1]>0){
        rate_p[1] = 1;
    }
    rate_p[2] = keep_ping[2] - ping[2];
    if(rate_p[2]<-5){
        rate_p[2] = -5;
    }else if(rate_p[2] >6){
        rate_p[2] = 6;
    }
    rate_p[3] = ping[3] - keep_ping[3];
    if(rate_p[3] <-10){
        rate_p[3] = -10;
    }else if(rate_p[3]>0){
        rate_p[3] = 1;
    }
    

}
*/
void home_stop0(){
    home_stop = 0;
}

void back_home(double *vx,double*vy){
    *vx = 0;
    *vy = -40;
    if(!home_stop){
        if(timer_home.read() ==0){
            timer_home.start();
        }
    }
    if(ping[2]<24){
        *vy = -35;
    }else if(ping[2]<30){
        *vy = -25;
    }
    if((abs(ping[1]-ping[3]))>25){
        if(ping[1]>ping[3]){
            //*vy = 15;
            *vx = -25;
        }else if(ping[1]<ping[3]){
            //*vy = 12;
            *vx = 25;
        }
    }
    
    //左角を抜ける方法を考える
    
    
    if((timer_home.read()>0.6)/*||home0.read()==0*/){
        *vy = 11;
        *vx = 0;
        timer_home.stop();
        timer_home.reset();
        home_stop = 1;
        home.attach(&home_stop0,BACK_HOME_TIME);
        //wait(0.1);
    }
    if(ping[2]<20){
        *vx = 0;
        *vy = 5;
    }

}
int main(void){
    
    uint8_t line[4] = {0};      /*@line_state,line_check*/
    double vx = 0,vy = 0;       /*used move(moter)*/
    int vs = 0, vk = 0;         /*used move(moter)*/
    int rateD = 0;              /*keep distance*/
    int rateDM  = 0;            /*keep_distrance to boal //ir_max use*/
    int rateP[4] = {0};         /*keep in front of goal*/
    void (*offense[10])(double *,double *,int *,int);
    void (*defense[10])(double *,double *,int *,int);
    
    //dcompass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    motor.baud(115200);                             //ボーレート設定
    motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
    motor.attach(&tx_motor,Serial::TxIrq);          //送信空き割り込み（モータ用）
    sensor.attach(&micon_rx,Serial::RxIrq);         //送信空き割り込み（センサ用）
    //compassdef = (compass / 10);                  //コンパス初期値保存
    //compassdef = (dcompass.sample() / 10);
    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    
    start.mode(PullDown);                           //digitalin pulldown
    check.mode(PullUp);                            //digitalin pullup
    kick_check.mode(PullUp);
    
    offense[0] = ir_offense0;
    offense[1] = ir_offense1;
    offense[2] = ir_offense2;
    offense[3] = ir_offense3;
    offense[4] = ir_offense4;
    offense[5] = ir_offense5;
    offense[6] = ir_offense6;
    offense[7] = ir_offense7;
    offense[8] = ir_offense8;
    offense[9] = ir_offense9;
    
    
    defense[0] = ir_defense0;
    defense[1] = ir_defense1;
    defense[2] = ir_defense2;
    defense[3] = ir_defense3;
    defense[4] = ir_defense4;
    defense[5] = ir_defense5;
    defense[6] = ir_defense6;
    defense[7] = ir_defense7;
    defense[8] = ir_defense8;
    defense[9] = ir_defense9;
    
    
    
    pidUpdata.attach(&PidUpdate, PID_CYCLE);
    //move(30,0,0,0);
    count = 0;
    if(check){// NOT use Xbee
        myled[3] = 0;
        count = 1;
        //lcd.printf("NOT USE XBee\nDEFENSE\n");
        /*
            count = 1:defense
            count = 0:offense
        */
    }else {
        myled[3] = 1;
        xbeerx.attach(&xbee_rx,5);
        xbeetx.attach(&xbee_tx,5);
    }
    myled[0] = 1; 
    while(!start){
        lcd.printf("comp %3d min %3dnum %3d main %3d",compass,ir_min,ir_num,ir_main);
        if(!kick_check){
            move(0,0,0,25);
        }else{
            move(0,0,0,0);
        }
    }
    myled[0] = 0;
    lcd.cls();
while(1){
        vx = 0, vy = 0, vk = 0;
        standTu = 0;

        if(ir_num<10){
            dist_fun_DM(&rateDM);//used defense first junction
            
            if(count){/****  DEFENSE ACTION!!  *******/
                if((rateDM > 0)||(ping[2]>33)){//key point
                    dist_fun_ping(rateP);//check ping state.
                    vx = DEFENSE_POWER*(goal_state1[rateP[1]+10][0]+goal_state3[rateP[3]+10][0]+ball_state0[ir_num][0]);    
                    vy = DEFENSE_POWER*(goal_state1[rateP[1]+10][1]+goal_state2[rateP[2]+5][1]+goal_state3[rateP[3]+10][1]+ball_state0[ir_num][1]);
                    if(ping[2]<28){
                        
                    }else if(ping[2]<45){
                        vy = -30;
                    }else {
                        vx =0;
                        vy = -35;
                    }
                    
                    
                }else{
                    dist_fun_pingSTRONG(rateP);
                    vx = DEFENSE_POWER*(apply[rateDM+7]*ball_state1[ir_num][0]+strongPing1[rateP[1]+10][0]+strongPing3[rateP[3]+10][0]);
                    vy = DEFENSE_POWER*(apply[rateDM+7]*ball_state1[ir_num][1]+strongPing1[rateP[1]+10][1]+strongPing2[rateP[2]+10][1]+strongPing3[rateP[3]+10][1]);
                    (*defense[ir_num])(&vx,&vy,&vk,rateD);
                    
                    if(ir_num == 2||ir_num == 3||ir_num == 4){
                        vk=25;
                    }
                }
                
                
            }else{/****  OFFENSE ACTION!!  *****/
            // if count =0
                if((rateDM > 0)&&((ir_num != 2)&&(ir_num !=3)&&(ir_num!=4))){ 
                        //アウトオブリーチ時後ろに下がりながらボールの直線上に移動する
                    dist_fun_ping(rateP);//check ping_state
                    
                    vx = OFFENSE_POWER*(goal_state1[rateP[1]+10][0]+goal_state3[rateP[3]+10][0]+ball_state0[ir_num][0]);    
                    vy = OFFENSE_POWER*(goal_state1[rateP[1]+10][1]+goal_state2[rateP[2]+5][1]+goal_state3[rateP[3]+10][1]+ball_state0[ir_num][1]);
                    
                    if(ping[2]<30){
                    //stop place
                    }else if(ping[2]<50){
                        vy = -30;
                    }else {
                        vx = 0;
                        vy = -35;
                    }
                    
                }else{/**** Main offense action */
                    
                    if(!dash_stop){
                        dist_fun_D(&rateD);//check ball state.
                        if(rateD > 0){
                            //dist_fun_pingSTRONG(rateP);
                            vx = OFFENSE_POWER*(near0[ir_num][0]+modulate_go[rateD+5]*go[ir_num][0]);
                            vy = OFFENSE_POWER*(near0[ir_num][1]+modulate_go[rateD+5]*go[ir_num][1]);
                        
                        }else{
                            vx = OFFENSE_POWER*(modulate_near[rateD+5]*near1[ir_num][0]+go[ir_num][0]);
                            vy = OFFENSE_POWER*(modulate_near[rateD+5]*near1[ir_num][1]+go[ir_num][1]);
                        }
                        
                        if(dash_charge.read()>=CHARGE_DACH_TIME){
                            dash_start.attach(&dash_reset,DASH_TIME);
                            dash_stop = 1;
                            //lcd.printf(" FALCON KICK \n\n");
                            dash_charge.stop();
                            dash_charge.reset();
                        }
                    }
                    (*offense[ir_num])(&vx,&vy,&vk,rateD);
                    if(dash_stop){
                        vk = 25;
                        vx = DASH_STRENGTH*way16[0][0];
                        vy = DASH_STRENGTH*way16[0][1];

                        
                    }
                        
                }
                
            }
        }
        
        if(ir_num >= 10){
            back_home(&vx,&vy);    
        }
        line_check(line);
        line_state(&vx,&vy,line);
        vs = compassPID;
        move(vx,vy,vs,vk);
        //lcd.printf("%0.2lf clock\n\n",clockL);
        //pc.printf("compassPID:%d\t compass:%d\n",s,compass);
        //pc.printf("%lf  %lf\n",x,y);
        //pc.printf("compass: %d\n",compass);
        //pc.printf("ping0:%d\tping1:%d\tping2:%d\tping3:%d\n",ping[0],ping[1],ping[2],ping[3]);
        //pc.printf("ir_min:%d\tir_num:%d\tir_main:%d\n",ir_min,ir_num,ir_main);
        //pc.printf("line0:%d\tline1:%d\tline2:%d\tline3:%d\n",adcline[0].read_u16(),adcline[1].read_u16(),adcline[2].read_u16(),adcline[3].read_u16());
    }
    
    
    
    
    
    
    
    /**************************************************
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    ****************************************************/
    /*
    while(1){
        vx = 0, vy = 0, vk = 0;
        standTu = 0;
        //dist_fun(&rateD,&rateDM,rateP);//3to-ri
        if(ir_num<10){*/
            /*if(rateD >= 0){//if(ir_num =2ka4) =3
                vx = STRENGTH*(modulate_near[rateD+5]*near0[ir_num][0]+modulate_go[rateD+5]*go[ir_num][0]);
                vy = STRENGTH*(modulate_near[rateD+5]*near0[ir_num][1]+modulate_go[rateD+5]*go[ir_num][1]);
            }else{
                vx = STRENGTH*(modulate_near[rateD+5]*near1[ir_num][0]+modulate_go[rateD+5]*go[ir_num][0]);
                vy = STRENGTH*(modulate_near[rateD+5]*near1[ir_num][1]+modulate_go[rateD+5]*go[ir_num][1]);
            }*/
            /*
            if(rateD >= 0){
                vx = STRENGTH*(modulate[rateD+5]*near0[ir_num][0]+modulate[rateD+5]*go[ir_num][0]);
                vy = STRENGTH*(modulate[rateD+5]*near0[ir_num][1]+modulate[rateD+5]*go[ir_num][1]);
            }else{
                vx = STRENGTH*(modulate[rateD+5]*near1[ir_num][0]+modulate_go[rateD+5]*go[ir_num][0]);
                vy = STRENGTH*(modulate[rateD+5]*near1[ir_num][1]+modulate_go[rateD+5]*go[ir_num][1]);
            }*/
            /*
            dist_fun_DM(&rateDM);//used defense first junction
            if(count){//defense
                if(rateDM > 0){//key point
                    dist_fun_ping(rateP);//check ping state.
                    vx = DEFENSE_POWER*(goal_state1[rateP[1]+10][0]+goal_state3[rateP[3]+10][0]+ball_state0[ir_num][0]);    
                    vy = DEFENSE_POWER*(goal_state1[rateP[1]+10][1]+goal_state2[rateP[2]+5][1]+goal_state3[rateP[3]+10][1]+ball_state0[ir_num][1]);
                    if(ping[2]<35){
                        
                    }else if(ping[2]<48){
                        vy = -30;
                    }else {
                        vy = 0;
                        vy = -35;
                    }
                }else{*/
                    /*if(ir_num<7){
                        vx = DEFENSE_POWER*ball_state1[ir_num][0];
                        vy = DEFENSE_POWER*(goal_state2[rateP[2]+5][1]+ball_state1[ir_num][1]);
                    }else if(ir_num<10){
                        vx = DEFENSE_POWER*(strongPing1[rateP[1]+10][0]+strongPing3[rateP[3]+10][0]+ball_state1[ir_num][0]);
                        vx = DEFENSE_POWER*(goal_state2[rateP[2]+5][1]+ball_state1[ir_num][1]);
                    }*/
                    /*
                    dist_fun_pingSTRONG(rateP);
                    vx = DEFENSE_POWER*(apply[rateDM+7]*ball_state1[ir_num][0]+strongPing1[rateP[1]+10][0]+strongPing3[rateP[3]+10][0]);
                    vy = DEFENSE_POWER*(apply[rateDM+7]*ball_state1[ir_num][1]+strongPing1[rateP[1]+10][1]+strongPing2[rateP[2]+10][1]+strongPing3[rateP[3]+10][1]);
                    if(ir_num == 2||ir_num == 3||ir_num == 4){
                        vk=25;
                    }
                }
            }else{//ofense
                if((rateDM > 0)&&((ir_num != 2)&&(ir_num !=3)&&(ir_num!=4))){
                    //アウトオブリーチ時後ろに下がりながらボールの直線上に移動する
                    dist_fun_ping(rateP);//check ping state.
                    vx = DEFENSE_POWER*(goal_state1[rateP[1]+10][0]+goal_state3[rateP[3]+10][0]+ball_state0[ir_num][0]);    
                    vy = DEFENSE_POWER*(goal_state1[rateP[1]+10][1]+goal_state2[rateP[2]+5][1]+goal_state3[rateP[3]+10][1]+ball_state0[ir_num][1]);
                    if(ping[2]<35){
                        
                    }else if(ping[2]<50){
                        vy = -30;
                    }else {
                        vx = 0;
                        vy = -35;
                    }
                }else{
                    
                    dist_fun_D(&rateD);//check ball state.
                    if(rateD > 0){
                        dist_fun_pingSTRONG(rateP);
                        vx = OFFENSE_POWER*(apply[rateDM+7]*near0[ir_num][0]+go[ir_num][0]);
                        vy = OFFENSE_POWER*(apply[rateDM+7]*near0[ir_num][1]+go[ir_num][1]);
                    }else{
                        vx = OFFENSE_POWER*(near1[ir_num][0]+go[ir_num][0]);
                        vy = OFFENSE_POWER*(near1[ir_num][1]+go[ir_num][1]);
                    }
                    (*offense[ir_num])(&vx,&vy,&vk,rateD);
                }
            }
        }
        if(ir_num >= 10){
            back_home(&vx,&vy);    
        }
        */    
        /*
        if(ir_num<10){
            (*offense[ir_num])(&vx,&vy,&vk);
            //vx =25*way10[ir_num][0],vy = 25*way10[ir_num][1];
        }else{
            clockL = 00.0;
            back_home(&x,&y);
        }
        
        */
        /*
        line_check(line);
        line_state(&vx,&vy,line);
        vs = compassPID;
        move(vx,vy,vs,vk);
        //lcd.printf("%0.2lf clock\n\n",clockL);
        //pc.printf("compassPID:%d\t compass:%d\n",s,compass);
        //pc.printf("%lf  %lf\n",x,y);
        //pc.printf("compass: %d\n",compass);
        //pc.printf("ping0:%d\tping1:%d\tping2:%d\tping3:%d\n",ping[0],ping[1],ping[2],ping[3]);
        //pc.printf("ir_min:%d\tir_num:%d\tir_main:%d\n",ir_min,ir_num,ir_main);
        //pc.printf("line0:%d\tline1:%d\tline2:%d\tline3:%d\n",adcline[0].read_u16(),adcline[1].read_u16(),adcline[2].read_u16(),adcline[3].read_u16());
     
     
     
     
     
     
     
     
     
     
     
     
     
     
     
        
    }*/
}