File content as of revision 2:59edff92b599:

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

#define MOTOR_P 30
#define NO_IR 45
#define LINE_LP 30
#define LINE_FP 30
#define LINE_ON 0xFFF0
#define LINE_TIME 0.5
#define R 1.0
#define S_MAX 5
#define S1 15
#define S2 10
#define S3 5
#define RATE 0.05
#define P_GAIN 0.8
#define I_GAIN 0.0
#define D_GAIN 0.02
//誤差errの範囲でvalue1がvalue2と等しければ0以外を、等しくなれば0を返す
#define ERR_EQ(value1,value2,err) ( ((value1) <= ((value2)+(err)))&&((value1) >= ((value2)-(err))) )

DigitalIn sw(p5);
DigitalIn start(p7);
DigitalOut myled[4] = {LED1, LED2, LED3, LED4};
Serial motor(p9,p10);
Serial sensor(p13,p14);
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 liner0;
Timeout liner1;
Timeout liner2;
Timeout liner3;
Ticker pidupdata;
//HMC6352 dcompass(p9,p10);

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

//uint16_t  analogHex[4] = {0};
int speed[4] = {0};
uint8_t value_ir = 0, ir_num = 0;
uint8_t ping[4] = {0};
uint8_t line[4] = {0}, line_stop[4] = {0};
uint8_t back = 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;

double way[8][2] = {
    { 0    , 1    },
    {-0.707, 0.707},
    {-1    , 0    },
    {-0.707,-0.707},
    { 0    ,-1    },
    { 0.707,-0.707},
    { 1    , 0    },
    { 0.707, 0.707}
};


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] = 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 line_state(){
    if(line[0]){
        y = -LINE_FP;
    }
    if(line[1]){
        x = LINE_LP;
    }
    if(line[2]){
        y = LINE_FP;
    }
    if(line[3]){
        x = -LINE_LP;
    }
}

void lcds(int lcd1){
    lcd.cls();
    lcd.locate(0, 0);
    lcd.printf("%d", lcd1);
    //lcd.locate(2, 1);
    //lcd.printf("YATTIYATTI");
}

void lcd_ping(){
    lcd.cls();
    lcd.locate(0,0);
    lcd.printf("%03d  %03d\n%03d  %03d", ping[FRONT], ping[LEFT], ping[BACK], ping[RIGHT]);
}

void lcd_line(){
    lcd.cls();
    lcd.locate(0,0);
    lcd.printf("%03d  %03d\n%03d  %03d", line[FRONT], line[LEFT], line[BACK], line[RIGHT]);
}


void home(){
    if(ping[LEFT] > 80){
        x = -20;
    } else if(ping[RIGHT] > 80){
        x = 20;
    }
    back = 30;
/*    if(EER_EQ(ping[LEFT]+ping[RIGHT],pingdef[LEFT]+pingdef[RIGHT], 5)){
        back = 10;
    }*/
    if(ping[BACK] > back){
        y = -20;
    }
}

void line_stop0(){
    line_stop[0] = 0;
}
void line_stop1(){
    line_stop[1] = 0;
}
void line_stop2(){
    line_stop[2] = 0;
}
void line_stop3(){
    line_stop[3] = 0;
}

void line_check()
{
    if(!line_stop[0]){
        if(adcline[0].read_u16() > LINE_ON){
            line[0] = 1;
            line_stop[0] = 1;
            myled[0] = 1;
            liner0.attach(&line_stop0,LINE_TIME);
        } else {
            line[0] = 0;
            myled[0] = 0;
        }
    }
    if(!line_stop[1]){
        if(adcline[1].read_u16() > LINE_ON){
            line[1] = 1;
            line_stop[1] = 1;
            myled[1] = 1;
            liner1.attach(&line_stop1,LINE_TIME);
        } else {
            line[1] = 0;
            myled[1] = 0;
        }
    }
    if(!line_stop[2]){
        if(adcline[2].read_u16() > LINE_ON){
            line[2] = 1;
            line_stop[2] = 1;
            myled[2] = 1;
            liner2.attach(&line_stop2,LINE_TIME);
        } else {
            line[2] = 0;
            myled[2] = 0;
        }
    }
    if(!line_stop[3]){
        if(adcline[3].read_u16() > LINE_ON){
            line[3] = 1;
            line_stop[3] = 1;
            myled[3] = 1;
            liner3.attach(&line_stop3,LINE_TIME);
        } else {
            line[3] = 0;
            myled[3] = 0;
        }
    }
}


void pidupdate()
{
    pidinput = compass;
    pid.setProcessValue(pidinput);
    
    compasspid = -pid.compute();
}

int main() {
    
    //dcompass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    uint8_t num = 0;
    
    wait(1);
    
    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(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.mode(PullUp);
    start.mode(PullUp);
    
    myled[0] = 1;
    while(start){}
    myled[0] = 0;
    
 /*   
    while(1){
        s = compasspid;
        pc.printf("%d\n", s);
        wait(0.1);
    }
    */

    while(1){
        line_check();
        
        //x = MOTOR_P;
        //y = 0;
        if(ir_num > 7){
            ir_num = 0;
        }
        x = MOTOR_P * way[ir_num][0];
        y = MOTOR_P * way[ir_num][1];
        s = compasspid;
        
        if(value_ir > NO_IR){
            home();
        }
        
        line_state();
        
        move(x,y,s);
    }
    while(1){
        line_check();
        //pc.printf("%d\n", adcline[1].read_u16());
        //pc.printf("%d %d %d %d\n", line[0], line[1], line[2], line[3]);
        if(ir_num > 7){
            ir_num = 0;
        }
        x = MOTOR_P;//MOTOR_P * way[ir_num][0];
        y = 0;//MOTOR_P * way[ir_num][1];
        s = (compass - 180) / 3;
        if(s > S1){
            s = S1;
        } else if(s < -S1){
            s = -S1;
        } else if(s > S2){
            s = S2;
        } else if(s < -S2){
            s = -S2;
        } else if(s > 0){
            s = S3;
        } else if(s < 0){
            s = -S3;
        }
                
        line_state();
        
        move(x,y,s);        
    }
    
/*    
    while(1){
        x = MOTOR_P * way[num][0];
        y = MOTOR_P * way[num][1];
        s = (compass - 180) / 3;
        if(s > S1){
            s = S1;
        } else if(s < -S1){
            s = -S1;
        } else if(s > S2){
            s = S2;
        } else if(s < -S2){
            s = -S2;
        } else if(s > S3){
            s = S3;
        } else if(s < -S3){
            s = -S3;
        }
        if(!sw){
            num++;
            wait(0.2);
            if(num > 7){
                num = 0;
            }
            lcds(num);
        }
        //pc.printf("%d\n", s);
        
        move(x,y,s);
        //wait(0.1);
    }
*/
/*
    while(1){
        x = 30;
        y = 0;
        move(x,y,s);
        wait(0.5);
        x = -30;
        y = 0;
        move(x,y,s);
        wait(0.5);
        //pc.printf("%d\n", s);
        //pc.printf("%d\n", adcline[3].read_u16());
    }
*/

/*    while(1){
        if(ir_num > 7){
            ir_num = 0;
        }
        x = MOTOR_P * way[ir_num][0];
        y = MOTOR_P * way[ir_num][1];
        s = (compass - 180) / 3;
        if(s > S_MAX){
            s = S_MAX;
        } else if(s < -S_MAX){
            s = -S_MAX;
        }
        
        move(x,y,s);
    }
    */    
/*    
    while(1){
        i = 3;
        analogHex[i] = adcIn[i].read_u16();
        if(analogHex[i] > 10000){
            myled[i] = 1;
            line[i] = 1;
            line_on = 1;
        } else {
            myled[i] = 0;
            line[i] = 0;
            line_on = 0;
        }
        
        x = 30;
        y = 0;
                
        if(line[1]){
            x = LINE_P;
        } else if(line[3]){
            x = -LINE_P;
        }
        
        if(line[0]){
            y = -LINE_P;
        } else if(line[2]){
            y = LINE_P;
        }
        move(x,y,s);
        
        if(line_on)wait(0.3);
    }
    */
    /*
    while(1){
        for(i = 0; i < 4; i++){
            analogHex[i] = adcIn[i].read_u16();
            if(analogHex[i] > 10000){
                myled[i] = 1;
                line[i] = 1;
            } else {
                myled[i] = 0;
                line[i] = 0;
            }
            //wait(0.1);
        }
        
        if(line[0]){
            x = -30;
        } else if(line[2]){
            x = 30;
        }
        
        if(line[1]){
            y = 30;
        } else if(line[3]){
            y = -30;
        }
        
        move(x,y);
        x = 0;
        y = 0;
        //pc.printf("%05d   %05d   %05d   %05d\n", analogHex[0], analogHex[1], analogHex[2],  analogHex[3]);
    }
    */
}