File content as of revision 3:69ce73ed70a7:

/* 取り敢えずこれで。 */

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

#define PAI 3.1415926535897932384626
#define POWER 0
#define PROPORTION 0.7
#define INSTANCE 1.05
#define LIMIT 1

BusOut m_led(LED1,LED2,LED3,LED4);
//DigitalOut myled(LED1);

Serial pc(USBTX,USBRX);
HMC6352 compass(p9,p10);
Serial      motor(p13,p14);//tx,rx
extern string StringFIN;
extern void array(int,int,int,int);
int speed[4] = {0};
int x = 0, y = 0,i=0,j=0;

//通信（モータ用）
void tx_motor(){
    array(speed[0],speed[1],speed[3],speed[2]);
    motor.printf("%s",StringFIN.c_str());
}

void move(double see,int direction){
    double pwm[4] = {0};
    
    pwm[0] = ( sin((see -  60.0)* PAI/180.0)*POWER )- direction * PROPORTION * LIMIT;
    pwm[1] = ( sin((see - 300.0)* PAI/180.0)*POWER )- direction * PROPORTION * LIMIT;
    pwm[2] = ( sin((see - 180.0)* PAI/180.0)*POWER )- direction * PROPORTION * LIMIT;
    pwm[3] = 0;
    if(direction<0){
    pwm[0] *= INSTANCE;   
    pwm[1] *= INSTANCE;
    pwm[2] *= INSTANCE; 
    }    
    //pc.printf("%d %d %d\n",speed[0],speed[1],speed[2]);
    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];
    }
}

int diffKun(int sampleC,int first){
    int diff=0;
    diff = sampleC + 540 - first;//0~180度の範囲で動かしたかった
    diff %= 360;
    diff -= 180;
    return diff;
}    

int main() {
    int seeta=0,diff=0,getFirst=0,min1=0,min2=1,max1=359,max2=358;
    int getCompass=0;
    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);//最初のSetting
    
    //
    getFirst = ( compass.sample() / 10.0 );
    
    seeta = 0;
    motor.baud(115200);                             //ボーレート設定
    motor.printf("1F0002F0003F0004F000\r\n");      //モータ停止     モーター番号(1~6) + ForR(前進後進) + パワー(0~100) 
    motor.attach(&tx_motor,Serial::TxIrq);         //送信空き割り込み（モータ用）
    
    //LEDは目印
    m_led = 0x01;
     wait(1);
    m_led = 0x03;
     wait(1);
     m_led = 0x07;
     wait(1);
    m_led = 0x0F;
    //
    
    while(1){
         getCompass = ( compass.sample() / 10.0 );
         diff = diffKun(getCompass,getFirst);
         
         move(seeta,diff); 
         pc.printf("%d %d  %d",getFirst,getCompass,diff);
         pc.printf("  %d %d %d\n",speed[0],speed[1],speed[2]);
         //wait(0.01);
    }
}