File content as of revision 0:d7ff86f25eaa:

#include "mbed.h"
#include "move.h"
#include "locate.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;
    update();
    px=coordinateX();
    py=coordinateY();
    //int 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()>PI/2) {
            move(0,0);
            break;
        }
    }
}

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

    update();
    pt=coordinateTheta();

    while(1) {
        update();
        if(pt-coordinateTheta()<(-1)*PI/2) {
            move(0,0);
            break;
        }
    }
}

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

    update();
    pt=coordinateTheta();

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

    hosei_turn(pt, true, PI/2);
}

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

    update();
    pt=coordinateTheta();

    while(1) {
        update();
        if(pt-coordinateTheta()<(-1)*PI/2) {
            move(0,0);
            break;
        }
    }

    hosei_turn(pt, false, PI/2);
}

void hosei_turn(float pt, bool cw, float rad)
{
    int np;
    if(cw) np = 1;
    else   np = -1;
    while(1) {
        update();
        if(pt-coordinateTheta() < np * rad - ALLOW_RAD) {
            move(-15, 15);
        } else if(pt-coordinateTheta() > np * rad + ALLOW_RAD) {
            move(15, -15);
        } else {
            move(0,0);
            return;
        }
    }
}

void turn_direction(int rad)
{
    move(turnspeed, (-1)*turnspeed);

    update();

    while(1) {
        update();
        if(coordinateTheta() < rad + 0.1 && coordinateTheta() > rad - 0.1) {
            move(0,0);
            break;
        }
    }

    hosei_turn(0, false, rad);        
}
void pmovex(int length)
{
    int px,py,ptheta,dx,dy,dtheta;
    int k_y=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
    int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。

    update();
    px=coordinateX();
    py=coordinateY();
    ptheta=coordinateTheta();
    move(rightspeed,leftspeed);

    while(1) {
        update();
        dx=coordinateX()-px;
        dy=coordinateY()-py;
        dtheta=coordinateTheta()-ptheta;

        if(dy>7) {
            dy=7;
        }
        if(dy<-7) {
            dy=-7;
        }
        move(rightspeed-k_y*dy-k_theta*dtheta,leftspeed+k_y*dy+k_theta*dtheta);

        if(dx>length) {
            move(0,0);
            break;
        }
    }
}

void pmovey(int length)
{
    int px,py,ptheta,dx,dy,dtheta;
    int k_x=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
    int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。

    update();
    px=coordinateX();
    py=coordinateY();
    ptheta=coordinateTheta();
    move(rightspeed,leftspeed);

    while(1) {
        update();
        dx=coordinateX()-px;
        dy=coordinateY()-py;
        dtheta=coordinateTheta()-ptheta;

        if(dx>7) {
            dx=7;
        }
        if(dx<-7) {
            dx=-7;
        }
        move(rightspeed-k_x*dx-k_theta*dtheta,leftspeed+k_x*dx+k_theta*dtheta);

        if(dy>length) {
            move(0,0);
            break;
        }
    }
}