20150217_133212

HMC5883Lで車輪ロボットの方向制御を行ない、図形(正方形)を描かせてみました。センサーは地磁気センサーのみですので移動距離は時間管理です。やはり角度の誤差が大きすぎてまともな描画はできないようです。
ペンを支えている白い部品は3Dプリンターで製作しました。これぐらいのものですと、設計20分、製作30分といったところです。金型品のような質感はありませんが気軽に作れるので重宝しています。
下記はmbedのコードです。イニシャライズ->キャリブレーション->走行、と動作モードにより動かします。地磁気センサーは-PIからPIの値が出力されますので出力範囲の両端をまたいで回転した場合(値が極端に変化)はそれをカウントして連続的に何回転でも計測できるように工夫しています。

#include "mbed.h"
#include "LPC11Uxx.h"
#include "HMC5883L.h"

#define STOP 0  //initial
#define CAL 1   //calibration
#define RUN 2   //run

HMC5883L compass(P0_5, P0_4);
Ticker ticker;

//mag sensor
double heading = 0.0;
double headingLPF = 0.0;
double initHeading;
double tgtHeading;
double difHeading = 0.0;
double prevHeading = 0.0;
int rotCnt = 0;
unsigned char m_mode;   //mag sensor mode

int maxX, minX, maxY, minY;
int ofsX = 0;                   //calibration x
int ofsY = 0;                   //calibration y
int counter = 0;

//draw action
unsigned char d_mode;   //draw mode
int task = 0;
int runCnt = 0;
int16_t iniRaw2,iniRaw0;

int params[][8] = {
  {0, 0, 50, 24, 0, 24, 0, 1500},
  {1, -157, 0, 24, 0, 0, 24, 1300},
  {0, -157, 50, 24, 0, 24, 0, 1500},
  {1, -314, 0, 24, 0, 0, 24, 1300},
  {0, -314, 50, 24, 0, 24, 0, 1500},
  {1, -471, 0, 24, 0, 0, 24, 1300},
  {0, -471, 50, 24, 0, 24, 0, 1500},
  {1, -628, 0, 24, 0, 0, 24, 1300},
  {0, -628, 50, 24, 0, 24, 0, 1500}
};

void sensHeading() {
    int16_t raw[3];

    compass.getXYZ(raw);
    prevHeading = heading;
    heading = atan2(static_cast<double>(raw[2]-ofsY), static_cast<double>(raw[0]-ofsX));
    if(counter == 0) {
        headingLPF = heading;
    }else {
        if(heading > (prevHeading + M_PI))rotCnt--;
        else if(heading < (prevHeading - M_PI))rotCnt++;
        headingLPF = 0.5*(heading + (rotCnt * 2* M_PI)) + 0.5*headingLPF;
    }
    switch(m_mode) {
    case STOP:
        if(counter == 100) {    //wait 2 seconds
            initHeading = headingLPF;
            m_mode = CAL;
            maxX = raw[0];
            minX = raw[0];
            maxY = raw[2];
            minY = raw[2];
            iniRaw0 = raw[0];
            iniRaw2 = raw[2];
            counter = 0;
        }
        break;
    case CAL:
        if(raw[0] > maxX)maxX = raw[0];
        if(raw[0] < minX)minX = raw[0];
        if(raw[2] > maxY)maxY = raw[2];
        if(raw[2] < minY)minY = raw[2];
        if(counter > 50 &&
            (raw[0]>iniRaw0-15 && raw[0]<iniRaw0+15) &&
            (raw[2]>iniRaw2-15 && raw[2]<iniRaw2+15)) {
            ofsX = (maxX + minX)/2;
            ofsY = (maxY + minY)/2;
            m_mode = RUN;
            counter = -1;
        }
        break;
    case RUN:
        if(counter == 0){
            rotCnt = 0;
        }
        break;
    }
    counter++;
}

void update() {
    sensHeading();
    switch(m_mode) {
        case STOP:
            LPC_CT16B0->MR0 = 0;  //duty0_right
            LPC_CT16B0->MR1 = 0;  //duty1_right
            LPC_CT16B1->MR0 = 0;  //duty0_left
            LPC_CT16B1->MR1 = 0;  //duty1_left
            break;
        case CAL:
            LPC_CT16B0->MR0 = 24;  //duty0_right
            LPC_CT16B0->MR1 = 0;  //duty1_right
            LPC_CT16B1->MR0 = 0;  //duty0_left
            LPC_CT16B1->MR1 = 24;  //duty1_left
            break;
        case RUN:
            d_mode = params[task][0];
            tgtHeading = params[task][1]*0.01 + initHeading;
            switch(d_mode){
                case 0:
                    difHeading = headingLPF - tgtHeading;
                    LPC_CT16B0->MR0 = 24+(difHeading*30.0);  //duty0_right
                    LPC_CT16B0->MR1 = 0;  //duty1_right
                    LPC_CT16B1->MR0 = 24-(difHeading*30.0);  //duty0_left
                    LPC_CT16B1->MR1 = 0;  //duty1_left
                    if(runCnt > params[task][2]){
                        LPC_CT16B0->MR0 = 0;  //duty0_right
                        LPC_CT16B0->MR1 = 0;  //duty1_right
                        LPC_CT16B1->MR0 = 0;  //duty0_left
                        LPC_CT16B1->MR1 = 0;  //duty1_left
                        runCnt = 0;
                        task++;
                    }
                    break;
                case 1:
                    difHeading = headingLPF - tgtHeading;
                    LPC_CT16B0->MR0 = 24;  //duty0_right
                    LPC_CT16B0->MR1 = 0;  //duty1_right
                    LPC_CT16B1->MR0 = 0;  //duty0_left
                    LPC_CT16B1->MR1 = 24;  //duty1_left
                    if(difHeading < 0){
                        LPC_CT16B0->MR0 = 0;  //duty0_right
                        LPC_CT16B0->MR1 = 0;  //duty1_right
                        LPC_CT16B1->MR0 = 0;  //duty0_left
                        LPC_CT16B1->MR1 = 0;  //duty1_left
                        runCnt = 0;
                        task++;
                    }
                break;
            }
            runCnt++;
            if(task>8)ticker.detach();
            //difHeading += 0.01;

            break;
        default:
        break;
    }
}

int main() {

    m_mode = STOP;
    compass.init();

    //PWM SETTINGS
    LPC_SYSCON->SYSAHBCLKCTRL |= (1<<7) |(1<<8);   //Enables clock for 16-bit counter/timer 0,1
    LPC_IOCON->PIO0_8 |= 2; // CT16B0_MAT0
    LPC_IOCON->PIO0_9 |= 2; // CT16B0_MAT1
    LPC_IOCON->PIO0_21 |= 1; // CT16B1_MAT0
    LPC_IOCON->PIO0_22 |= 2; // CT16B1_MAT1
    LPC_CT16B0->MCR = 1<<10; // Reset on MR3: the TC will be reset if MR3 matches it
    LPC_CT16B1->MCR = 1<<10; // Reset on MR3: the TC will be reset if MR3 matches it
    LPC_CT16B0->MR0 = 0;  //duty0_right
    LPC_CT16B0->MR1 = 0;  //duty1_right
    LPC_CT16B1->MR0 = 0;  //duty0_left
    LPC_CT16B1->MR1 = 0;  //duty1_left
    LPC_CT16B0->MR3 = 256;  //period 48MHz/12/256=15625Hz
    LPC_CT16B1->MR3 = 256;  //period
    LPC_CT16B0->PR = 3;    //12-1
    LPC_CT16B1->PR = 3;    //12-1
    LPC_CT16B0->PWMC |= (1<<0) | (1<<1);  //PWM mode is enabled for CT32B0_MAT0,1
    LPC_CT16B1->PWMC |= (1<<0) | (1<<1);  //PWM mode is enabled for CT32B1_MAT0,1
    LPC_CT16B0->TCR = 1;    //The Timer Counter and Prescale Counter are enabled for counting
    LPC_CT16B1->TCR = 1;    //The Timer Counter and Prescale Counter are enabled for counting

    wait(2);
    ticker.attach(&update, 0.02);

    while(1) {
    }
}