File content as of revision 4:4c574be6325c:

#include "mbed.h"
#include "move.h"
#include "locate.h"
#include "stdlib.h"

PwmOut M1cw(PA_11);
PwmOut M1ccw(PB_15);
PwmOut M2ccw(PB_14);
PwmOut M2cw(PB_13);
/*
DigitalOut teamledblue(PC_10);
DigitalOut teamledred(PC_12);
*/

//Serial pc(SERIAL_TX, SERIAL_RX);    //pcと通信


//const int allowlength=5;
//const float allowdegree=0.02;
const int rightspeed=120;
const int leftspeed=rightspeed + 2;
const int turnspeed=30*2;
const float k = 0.9;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
const int k_theta = 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 hosei_turn(float pt, bool cw, float rad)
{
    int np;
    if(cw) np = 1;
    else   np = -1;
    while(1) {
        update();
        //pc.printf("t:%f\n\r", coordinateTheta());
        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();
        //pc.printf("t:%f\n\r", coordinateTheta());
        if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) {
            move(0,0);
            break;
        }
    }



    hosei_turn(0, false, rad);

    wait(0.5);

    hosei_turn(0, false, rad);

    wait(0.5);
}


void pmove_to_dir(int direction, int x, int y)
{
    float k = 0.9;
    int   k_theta = 2;

    int length, dx, dy;
    int *d_length, *disorder;
    float dtheta, ptheta;
    float daikei;

    update();

    switch(direction) {
        case X_PLUS:
            length = x - coordinateX();
            y = coordinateY();
            d_length = &dx;
            disorder = &dy;
            turn_abs_rad(0);

            ptheta = 0;
            break;
        case Y_PLUS:
            length = y - coordinateY();
            x = coordinateX();
            d_length = &dy;
            disorder = &dx;
            k *= -1;
            turn_abs_rad(PI/2);
            ptheta = PI/2;
            break;
        case X_MINUS:
            length = x - coordinateX();
            y = coordinateY();
            d_length = &dx;
            disorder = &dy;
            k *= -1;
            turn_abs_rad(PI);
            //pc.printf("finish_turn");
            ptheta = PI;
            break;
        case Y_MINUS:
            length = y - coordinateY();
            x = coordinateX();
            d_length = &dy;
            disorder = &dx;
            turn_abs_rad(PI*3/2);
            ptheta = PI*3/2;
            break;
        default:
            return;
    }

    if(length == 0) return;

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


        if(*disorder>1) {
            *disorder = 1;
        } else if(*disorder<-1) {
            *disorder = -1;
        }


        if(abs(*d_length) > abs(length) -200) {
            daikei = (float)(abs(length)-abs(*d_length)) / 200;
        } else if(abs(*d_length) < 150) {
            daikei = (float)(abs(*d_length)) / 150;
        } else
            daikei = 1;

        move(daikei * (rightspeed*7/8.0 - k*(*disorder) - k_theta*dtheta) + rightspeed/8.0, daikei * (leftspeed*7/8.0 + k*(*disorder) + k_theta*dtheta) + leftspeed/8.0);

        if((direction > 0 && *d_length >= 0) || (direction < 0 &&  *d_length <= 0)) {
            move(0, 0);
            break;
        }

        //pc.printf("d_length:%d disorder:%d rs:%f ls:%f g:%f\n\r", *d_length, *disorder, (rightspeed/2.0 - k*(*disorder) - k_theta*dtheta) + rightspeed/2.0, (leftspeed/2.0 + k*(*disorder) + k_theta*dtheta) + leftspeed/2.0, daikei);
    }

    wait(0.5);
}

void back(int x, int y)
{ 
    float k = 0.9;
    int   k_theta = 2;

    int length, dx, dy;
    int *d_length, *disorder;
    int absd_length;
    float dtheta, ptheta;
    float daikei;

    char direction;
    if(abs(x - coordinateX()) > abs(y - coordinateY())) {
        y = coordinateY();
        direction = X_PLUS;
        length = abs(x - coordinateX());
        d_length = &dx;
        disorder = &dy;
    }
    else {
        x = coordinateX();
        direction = Y_PLUS;
        length = abs(y - coordinateY());
        d_length = &dy;
        disorder = &dx;
    }

    if(d_length < 0)
        direction *= -1;

    switch(direction) {
        case X_PLUS:
            ptheta = PI;
            break;
        case Y_PLUS:
            k *= -1;
            ptheta = PI*3/2;
            break;
        case X_MINUS:
            k *= -1;
            ptheta = 0;
            break;
        case Y_MINUS:
            ptheta = PI/2;
            break;
        default:
            return;
    }

    turn_abs_rad(ptheta);

    if(length == 0) return;

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

        if(*disorder>1) {
            *disorder = 1;
        } else if(*disorder<-1) {
            *disorder = -1;
        }

        absd_length = abs(*d_length);

        if(absd_length > length - 30) {
            daikei = -(length - absd_length) / 30.0;
        } else if(absd_length < 150) {
            daikei = -absd_length / 150.0;
        } else
            daikei = -1;

        move(daikei * (rightspeed*6/7.0 - k*(*disorder) + k_theta*dtheta) - rightspeed/7.0,
             daikei * (leftspeed*6/7.0  + k*(*disorder) - k_theta*dtheta) - leftspeed/7.0);

        if((direction > 0 && *d_length <= 0) || (direction < 0 &&  *d_length >= 0)) {
            move(0, 0);
            break;
        }

        //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
    }

    wait(0.5);
    
}