File content as of revision 1:405e28b64fdb:

#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 k_x = 0.9;
const float k_y = 0.9;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
const int k_theta = 500;
//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()
{
    update();
    float pt = coordinateTheta();;
    move(-turnspeed, turnspeed);

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

void turnccw()
{
    update();
    float pt = coordinateTheta();;
    move(turnspeed, -turnspeed);

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

void turn_cw()
{
    update();
    float pt = coordinateTheta();;
    move(-turnspeed, turnspeed);

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

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

void turn_ccw()
{
    update();
    float pt = coordinateTheta();;
    move(turnspeed, -turnspeed);

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

    hosei_turn(pt, CCW, 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_abs_rad(float rad)
{
    update();

    int np;
    if(coordinateTheta() > rad) np = 1;
    else if(coordinateTheta() < rad) np = -1;
    else return;

    move((-np)*turnspeed, (np)*turnspeed);

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

    hosei_turn(0, false, rad);
}


void pmovex(int length)
{
    int px,py,ptheta,dx,dy,dtheta;

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

    while(1) {
        update();
        dx=coordinateX()-px;
        dy=coordinateY()-py;
        dtheta=coordinateTheta()-ptheta;
        if(dy>3) {
            dy=3;
        } else if(dy<-3) {
            dy=-3;
        }
        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;

    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;
        } else 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;
        }
    }
}

/*
void pmovex_minus(int length)
{
    int px,py,ptheta,dx,dy,dtheta;

    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_minus()
{
    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;
        }
    }
}
*/

void pmovex_to(int x, int y)
{
    int px,py,ptheta,dx,dy,dtheta;

    if(x <= coordinateX()) return;
    update();
    py=y;
    ptheta=coordinateTheta();
    move(rightspeed,leftspeed);

    while(1) {
        update();
        dy=coordinateY()-py;
        dtheta=coordinateTheta()-ptheta;
        if(dy>3) {
            dy=3;
        } else if(dy<-3) {
            dy=-3;
        }
        move(rightspeed - k_y * dy - k_theta * dtheta,leftspeed + k_y *dy + k_theta * dtheta);

        if(coordinateX() > x) {
            move(0,0);
            break;
        }
    }
}

/*
void pmovey_to(int x, int y)
{
    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;
        } else 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;
        }
    }
}

void pmovex_minus_to(int x, int y)
{
    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_minus_to(int x, int y)
{
    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;
        }
    }
}
*/