File content as of revision 5:6604ec9044a0:

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

#define MOTOR_P 35
#define HOME_P 30
#define TURN_P 20
#define BALL_P 25
#define NO_BALL 45
#define LINE_P 30
#define LINE_PLR 17
#define LINE_ON 0x2710  //10000
#define LINE_TIME 0.6
#define LINE_PING 40
#define R 1.0
#define S_MAX 5
#define S1 15
#define S2 10
#define S3 5
#define RATE 0.05
#define P_GAIN 1.7
#define I_GAIN 0.0
#define D_GAIN 0.013
#define HOME_BACK 30
#define HOME 1
#define KICK 25
#define GOAL 30.0
#define PING_LR 100
#define BALL_FAR 43
#define BALL_NEAR 40

DigitalIn sw_wh(p5);
DigitalIn start(p7);
DigitalOut myled[4] = {LED1, LED2, LED3, LED4};
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] = {p16, p17, p19, p20};
PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);
Timeout liner_F;
Timeout liner_L;
Timeout liner_B;
Timeout liner_R;
Ticker pidupdata;
Ticker xbeetx;
Ticker xbeerx;
//HMC6352 dcompass(p9,p10);

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

//uint16_t  analogHex[4] = {0};
int speed[4] = {0};
uint8_t value_ir = 0, ir_num = 0, ir_dis = 0, ball_on = 0;
uint8_t ping[4] = {0};
uint8_t line[4] = {0}, line_stop[4] = {0}, line_ping[4] = {0};
uint8_t kick = 0;
//uint8_t robot_state = 0;
int compass = 0;
int x = 0, y = 0, s = 0, i = 0, line_on = 0;
int compassdef = 0, data = 0;
uint8_t pingdef[4] = {0};

double pidinput = 180.0;
double compasspid = 0.0;
double goal = 0.0;

double way[10][2] = {
    { 0    , 1    },    //FF
    {-0.707, 0.707},    //FL
    {-1    , 0    },    //LL
    {-0.707,-0.707},    //BL
    { 0    ,-1    },    //BB
    { 0.707,-0.707},    //BR
    { 1    , 0    },    //RR
    { 0.707, 0.707},    //FR
    {-0.500, 0.866},    //FFL
    { 0.500, 0.866}     //FFR
};
double ball_way[12][2] = {
    { 0.0  , 1.0  },    //FF
    {-0.866,-0.500},    //FL
    {-0.342,-0.940},    //LL
    { 0.342,-0.940},    //BL
    { 0.966, 0.259},    //BB
    {-0.342,-0.940},    //BR
    { 0.342,-0.940},    //RR
    { 0.985,-0.173},    //FR
    {-0.985, 0.174},    //FFL
    { 0.985, 0.174},    //FFR
    {-0.173, 0.985},    //FFFL
    { 0.173, 0.985}     //FFFR
};

void move(int vx, int vy, int vs){
    double pwm[4] = {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] = kick;

    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(){    
    if(line[LEFT]){
        x = LINE_P;
    } else if(line[RIGHT]){
        x = -LINE_P;
    }
        
    if(line[FRONT]){
        if(line[LEFT] | line[RIGHT]){
            y = -LINE_PLR;
        } else {
            y = -LINE_P;
        }
    } else if(line[BACK]){
        if(line[LEFT] | line[RIGHT]){
            y = LINE_PLR;
        } else {
            y = LINE_P;
        }
    }
}

void line_stop_F(){
    line_stop[FRONT] = 0;
}
void line_stop_L(){
    line_stop[LEFT] = 0;
}
void line_stop_B(){
    line_stop[BACK] = 0;
}
void line_stop_R(){
    line_stop[RIGHT] = 0;
}

void line_check()
{
    if(!line_stop[FRONT]){
        if(adcline[FRONT].read_u16() > LINE_ON){
            line[FRONT] = 1;
            line_stop[FRONT] = 1;
            line_stop[BACK] = 1;
            //line_ping[FRONT] = 1;
            myled[0] = 1;
            liner_F.attach(&line_stop_F,LINE_TIME);
            liner_B.attach(&line_stop_B,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;
            //line_ping[LEFT] = 1;
            myled[1] = 1;
            liner_L.attach(&line_stop_L,LINE_TIME);
            liner_R.attach(&line_stop_R,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;
            //line_ping[BACK] = 1;
            myled[2] = 1;
            liner_B.attach(&line_stop_B,LINE_TIME);
            liner_F.attach(&line_stop_F,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;
            //line_ping[RIGHT] = 1;
            myled[3] = 1;
            liner_R.attach(&line_stop_R,LINE_TIME);
            liner_L.attach(&line_stop_L,LINE_TIME);
        } else {
            line[RIGHT] = 0;
            myled[3] = 0;
        }
    }
}


void pidupdate()
{
    //pid.setSetPoint(180.0 + goal);
    pidinput = compass;
    pid.setProcessValue(pidinput);
    
    compasspid = -pid.compute();
}

void home()
{
    x = 0;
    y = 0;
    kick = 0;
    goal = 0.0;
    
  //  if((145 < ping[LEFT]+ping[RIGHT]) && (ping[LEFT]+ping[RIGHT] < 155)){    
        if(ping[LEFT] > PING_LR){
            x = -HOME_P;
            y = -5;
        } else if(ping[RIGHT] > PING_LR){
            x = HOME_P;
            y = -5;
        }
  //  }

    if(ping[BACK] > HOME_BACK){
        y = -HOME_P;
    } else if(ping[BACK] < 5){
        y = HOME_P;
    }
}

int main() {
    
    //dcompass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    //uint8_t num = 0;
    uint8_t dir = 0, power = 0, lcd_count = 0;
    int x_ball = 0, x_turn = 0, y_ball = 0, y_turn = 0;
    
    wait(1);
    
    motor.baud(115200);                             //ボーレート設定
    motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
    motor.attach(&tx_motor,Serial::TxIrq);          //送信空き割り込み（モータ用）
    sensor.attach(&micon_rx,Serial::RxIrq);         //送信空き割り込み（センサ用）
    xbeerx.attach(&xbee_rx,5);
    xbeetx.attach(&xbee_tx,5);
    //compassdef = (compass / 10);           //コンパス初期値保存
    //compassdef = (dcompass.sample() / 10);
    pid.setInputLimits(0.0,360.0);
    pid.setOutputLimits(-30.0,30.0);
    pid.setBias(0.0);
    pid.setMode(AUTO_MODE);
    pid.setSetPoint(180.0);
    pidupdata.attach(&pidupdate, 0.05);
    
    sw_wh.mode(PullUp);
    start.mode(PullDown);
    
    lcd.cls();
    lcd.locate(0,0);
    lcd.printf("Chronograph");    
    
    
    //move(x,y,s);
    
    myled[0] = 1;
    
    //スタート前チェック
    while(!start){
        if(lcd_count == 0){
            lcd.locate(6,1);
            lcd.printf("%3d", compass);
        } else if(lcd_count == 1){
            lcd.locate(0,1);
            lcd.printf("ir_dis = %3d", ir_dis);
        } else if(lcd_count == 2){
            lcd.locate(0,0);
            lcd.printf("ir_num = %3d\nir_val = %3d", ir_num, value_ir);
        } else if(lcd_count == 3){
            lcd.locate(0,0);
            lcd.printf("F:%3d   B:%3d\nL:%3d   R:%3d", ping[FRONT], ping[BACK], ping[LEFT], ping[RIGHT]);
        } else if(lcd_count == 4){
            lcd.locate(0,0);
            lcd.printf("F:%5d B:%5d\nL:%5d R:%5d", adcline[FRONT].read_u16(), adcline[BACK].read_u16(), adcline[LEFT].read_u16(), adcline[RIGHT].read_u16());
        }
        if(!sw_wh){
            lcd_count++;
            if(lcd_count == 5){
                lcd_count = 0;
                lcd.cls();
                lcd.locate(0,0);
                lcd.printf("Chronograph");
            }
            wait(0.5);
        }
        wait(0.1);
    }
    
    
    
    
    myled[0] = 0;
    lcd.cls();
    lcd.locate(0,0);
    lcd.printf("Chronograph");
    lcd.locate(7,1);
    lcd.printf("ABURASOBA");
    
    
    
    while(1){
        x = 0;
        y = 0;
        s = compasspid;        
        power = MOTOR_P;
        
        if((ir_dis < 35)/* && ( (ir_num == FF) || (ir_num == FFL) || (ir_num == FFR) )*/){
            kick = KICK;                
        } else {
            kick = 0;
            //goal = 0.0;
        }
        
        if((ir_num != FF) && (ir_num != FFL) && (ir_num != FFR)){
            if(ir_dis > BALL_FAR){
                x_turn = TURN_P * way[ir_num][0];
                y_turn = TURN_P * way[ir_num][1];
                
                x_ball = BALL_P * ball_way[ir_num][0];
                y_ball = BALL_P * ball_way[ir_num][1];
                
                if(ir_num == BB){
                    if(ping[LEFT] > ping[RIGHT]){
                        x_ball = -x_ball;
                    }
                }
                
                x = x_turn + x_ball;
                y = y_turn + y_ball;
                
            } else if(ir_dis < BALL_NEAR){
                x_turn = -(TURN_P - 5) * way[ir_num][0];
                y_turn = -(TURN_P - 5) * way[ir_num][1];
                
                x_ball = BALL_P * ball_way[ir_num][0];
                y_ball = BALL_P * ball_way[ir_num][1];
                
                if(ir_num == BB){
                    if(ping[LEFT] > ping[RIGHT]){
                        x_ball = -x_ball;
                    }
                }
                
                x = x_turn + x_ball;
                y = y_turn + y_ball;
                
            } else {
                if((ir_num == FL) || (ir_num == FR)){
                    power = 20;
                }
                x = power * ball_way[ir_num][0];
                y = power * ball_way[ir_num][1];
                if(ir_num == BB){
                    if(ping[LEFT] > ping[RIGHT]){
                        x = -x;
                    }
                }
            }
            
        } else {            
            if(ir_num == FF){
                power = MOTOR_P + 10;
            }
            x = power * ball_way[ir_num][0];
            y = power * ball_way[ir_num][1];
            
        }
       
        line_check();
        
        if((ir_num == NO_BALL)/* || (ball_on < 40)*/){
            home();
        }

        //x = 30; y = 10;
        line_state();
        
        if((x == 0) && (y == 0)){
            if(ping[BACK] > 25){
                y = -HOME_P;
            }
        }
        
        //x = 0; y = 0;
        
        move(x,y,s);
    }
    
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////    

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////    

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    
    while(1){
        
        x = 0;
        y = 0;
        s = compasspid;        
        power = MOTOR_P;
        
        if(/*(ir_dis < 60) && */( (ir_num == FF) || (ir_num == FFL) || (ir_num == FFR) )){
            //kick = 25;
            //if(130 < ping[LEFT]+ping[RIGHT]){
                //if(goal == 0){
                   /* if(ping[LEFT] > 100){
                        goal = -GOAL;
                    }*//* else if(ping[RIGHT] > 100){
                        //goal = GOAL;
                    }*/
                //}
            //}
                
        } else {
            kick = 0;
            goal = 0.0;
        }
        
        if((ir_dis > 130) && (ir_dis < 150) && (ir_num != BB) && (ir_num != FL) && (ir_num != FR) && (ir_num != BL) && (ir_num != BR)){
            x = power * way[ir_num][0];
            y = power * way[ir_num][1];
        } else {            
            if((ir_num == FF) || (ir_num == FFFL) || (ir_num == FFFR)){
                power = MOTOR_P + 10;
            } else if((ir_num == FFL) || (ir_num == FFR)){
                power = MOTOR_P;
            } else {
                power = MOTOR_P + 5;
            }
            x = power * ball_way[ir_num][0];
            y = power * ball_way[ir_num][1];
            if(ir_num == BB){
                if(ping[LEFT] > ping[RIGHT]){
                    x = -x;
                }
            }
        }
       
        line_check();
        
        if((ir_num == NO_BALL)/* || (ball_on < 40)*/){
            home();
        }

        //x = 30; y = 10;
        line_state();
        
        if((x == 0) && (y == 0)){
            if(ping[BACK] > 25){
                y = -HOME_P;
            }
        }
        
        //x = 0; y = 0;
        
        move(x,y,s);
    }
}