File content as of revision 0:e6d14fec4954:

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

#define LINE_P 70
#define R 1.0

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 adcIn[4] = {p17, p18, p19, p20};
//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 ping[4] = {0};
uint8_t line[4] = {0};
uint8_t ir_max = 0, ir_num = 0;
int compass = 0;
int x = 0, y = 0, s = 0, i = 0, line_on = 0;
int compassdef = 0, data = 0;


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_P;
    }
    if(line[1]){
        x = LINE_P;
    }
    if(line[2]){
        y = LINE_P;
    }
    if(line[3]){
        x = -LINE_P;
    }
}

int main() {
    
    //dcompass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
    
    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);
    

    wait(1);

    //while(1);
    
    y = 20;
    
    while(1){
        s = (compass - 180) / 5;
        //pc.printf("%d\n", s);
        
        move(x,y,s);
        //wait(0.1);
    }
/*    
    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]);
    }
    */
}