File content as of revision 2:89bdaf269693:

/* 実験中 */

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

#define PAI 3.1415926535897932384626
#define POWER 0
#define LIMIT 1
#define TRIALS 1
#define ZERO 0

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 * 0.7;
    pwm[1] = ( sin((see - 300.0)* PAI/180.0)*POWER )- direction * 0.7;
    pwm[2] = ( sin((see - 180.0)* PAI/180.0)*POWER )- direction * 0.7;
    pwm[3] = 0;
    pwm[0] *= LIMIT;
    pwm[1] *= LIMIT;
    pwm[2] *= LIMIT;
    //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 sampleC[TRIALS] = {0},getCompass[TRIALS] = {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){
         //
         /*for(j=0;j<TRIALS;j++){
            getCompass[j] = ( compass.sample() / 10.0 );
            //pc.printf("%d\n",getCompass[j]);
            getCompass[j] = diffKun(getCompass[j],getFirst);
            if((getCompass[j]>359)&&(j<0)) getCompass[j] = getCompass[j-1];
            diff += getCompass[j];  
         }
         diff /= TRIALS;
         diff = diffKun(diff,getFirst);
         pc.printf("%d\n",diff);*/
         //
         getCompass[0] = ( compass.sample() / 10.0 );
         diff = diffKun(getCompass[0],getFirst);
         
         move(seeta,diff); 
         pc.printf("%d %d  %d",getFirst,getCompass[0],diff);
         pc.printf("  %d %d %d\n",speed[0],speed[1],speed[2]);
         //wait(0.01);
    }
    /*wait(1); 
    speed[0] = 0;
    speed[1] = 0;
    speed[2] = 0;*/ 
    while(1);
}