File content as of revision 6:26ac9f539e8b:

#include "mbed.h"
#include "locate.h"
#include "math.h"
PwmOut M1cw(PA_11);
PwmOut M1ccw(PB_15);
PwmOut M2ccw(PB_14);
PwmOut M2cw(PB_13);

const int allowlength=5;
const float allowdegree=0.02;
const int rightspeed=29*2;
const int leftspeed=31*2;
const int turnspeed=15*2;

const float PIfact=2.89;

void initmotor()
{


    M1cw.period_us(256);
    M1ccw.period_us(256);
    M2cw.period_us(256);
    M2ccw.period_us(256);

}

void move(int left,int right)
{

    float rightduty,leftduty;

    if(right>256) {
        right=256;
    }
    if(left>256) {
        left=256;
    }
    if(right<-256) {
        right=-256;
    }
    if(left<-256) {
        left=-256;
    }

    rightduty=right/256.0;
    leftduty=left/256.0;
    if(right>0) {
        M1cw.write(1-rightduty);
        M1ccw.write(1);
    } else {
        M1cw.write(1);
        M1ccw.write(1+rightduty);
    }

    if(left>0) {
        M2cw.write(1-leftduty);
        M2ccw.write(1);
    } else {
        M2cw.write(1);
        M2ccw.write(1+leftduty);
    }
}


void movelength(int length)
{
    int px,py,pt;
    update();
    px=coordinateX();
    py=coordinateY();
    pt=coordinateTheta();

    move(rightspeed,leftspeed);
    while(1) {

        update();
        //pc.printf("dx:%d, dy:%d, l:%d x:%d y:%d t:%f\n\r",px-coordinateX(),py-coordinateY(),length,coordinateX(),coordinateY(), coordinateTheta());
        if(((px-coordinateX())*(px-coordinateX())+(py-coordinateY())*(py-coordinateY()))>length*length) {
            break;
        }

    }
    move(0,0);
}

void turncw()
{
    float pt;
    move((-1)*turnspeed,turnspeed);

    update();
    pt=coordinateTheta();

    while(1) {
        update();
        if(pt-coordinateTheta()>PIfact/2) {
            move(0,0);
            break;
        }
    }
}

void turnccw()
{
    float pt;
    move(turnspeed,(-1)*turnspeed);

    update();
    pt=coordinateTheta();

    while(1) {
        update();
        if(pt-coordinateTheta()<(-1)*PIfact/2) {
            move(0,0);
            break;
        }
    }
}
/*
int main(){
    setup();
    initmotor();

    for(int i=0; i < 4; i++)
    {
        movelength(1200);
        turnccw();
    }
}*/

void pmovex(int length){
    int px,py,dx,dy;
    int k=1;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
    update();
    px=coordinateX();
    py=coordinateY();
    move(rightspeed,leftspeed);
        
    while(1){
        update();
        dx=coordinateX()-px;
        dy=coordinateY()-py;

        if(dy>7){dy=7;}
        if(dy<-7){dy=-7;}
        move(rightspeed-k*dy,leftspeed+k*dy);
        
        if(dx>length){
            move(0,0);
            break;
        }
    }
}

int main()
{
    setup();
    initmotor();
    
    movelength(600);
    turnccw();
    movelength(900);
    turnccw();
    movelength(600);

}