File content as of revision 12:4e9cadc225f5:

#include "mbed.h"
#include "move.h"
#include "locate.h"
#include "stdlib.h"
#include "math.h"
 
PwmOut M1cw(PA_11);
PwmOut M1ccw(PB_15);
PwmOut M2ccw(PB_14);
PwmOut M2cw(PB_13);
 
DigitalOut green (PC_2);
DigitalOut yellow(PC_3);
DigitalOut red   (PC_0);
 
/*
DigitalOut teamledblue(PC_10);
DigitalOut teamledred(PC_12);
*/
 
Serial pc2(SERIAL_TX, SERIAL_RX);    //pcと通信
 
 
//const int allowlength=5;
//const float allowdegree=0.02;
const int rightspeed=70;
const int leftspeed=rightspeed + 4;
const int hosei_ = 13;
const int max_disorder = 4;
const float ratio = 1.0/7.5;
//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_np();
        //pc.printf("t:%f\n\r", coordinateTheta());
        if(pt-coordinateTheta() < np * rad - ALLOW_RAD) {
            move(-hosei_, hosei_);
        } else if(pt-coordinateTheta() > np * rad + ALLOW_RAD) {
            move(hosei_, -hosei_);
        } else {
            move(0,0);
            return;
        }
    }
 
}
 
void turnrad(float rad)
{
    green = 1;
 
    update_np();
 
    int np;
    if(coordinateTheta() > rad) np = 1;
    else if(coordinateTheta() < rad) np = -1;
    else return;
 
    move((-np)*rightspeed, (np)*leftspeed);
 
    while(1) {
        update_np();
        //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);
    green = 0;
 
}
 
 
void turnrad_ccw(float rad)
{
    green = 1;
 
    update();
 
    move(rightspeed, -leftspeed);
 
    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);
    green = 0;
}
 
void turnrad_cw(float rad)
{
    green = 1;
 
    update();
 
    move((-1)*rightspeed, leftspeed);
 
    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.2);
 
    hosei_turn(0, false, rad);
 
    wait(0.2);
    green = 0;
}
 
void pmove(int x, int y)
{
    yellow = 1;
 
    float k = 1.0;//ズレ(mm)を回転数に反映させる比例定数
    int   k_theta = 25;//ズレ(rad)を回転数に反映させる比例定数
 
    int length, dx, dy;
    int *d_length, *disorder;
    int absd_length;
    float dtheta, ptheta;
    float daikei;
 
    int 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;
    }
 
    update();
    dx = x - coordinateX();
    dy = y - coordinateY();
 
    if(*d_length < 0)   //x,y減少方向なら、*d_length<0
        direction *= -1;
 
    pc2.printf("direction:%d", direction);
 
    switch(direction) {
        case X_PLUS:
            ptheta = 0;
            break;
        case Y_PLUS:
            k *= -1;
            ptheta = PI/2;
            break;
        case X_MINUS:
            k *= -1;
            ptheta = PI;
            break;
        case Y_MINUS:
            ptheta = -PI/2;
            break;
        default:
            return;
    }
 
    ptheta += nearPi(coordinateTheta() - ptheta);
 
    turnrad(ptheta);
 
    if(length == 0) return;
 
    int i = 0;
 
    while(1) {
        update_np();
        dx = x - coordinateX();
        dy = y - coordinateY();
        dtheta = coordinateTheta() - ptheta;
 
        if(*disorder>max_disorder) {
            *disorder = max_disorder;
        } else if(*disorder<-max_disorder) {
            *disorder = -max_disorder;
        }
 
        absd_length = abs(*d_length);
 
 
        if(i++ < 5) {
            daikei = i/5;
        } else if(absd_length < 300) {
            daikei = absd_length / 300.0;
        }
        /*
                else if(absd_length > length - 30) {
                    daikei = abs(length - absd_length) / 30.0;
        */
        else
            daikei = 1;
 
        move(daikei * (rightspeed*(1-ratio) + k*(*disorder) - k_theta*dtheta) + rightspeed*ratio,
             daikei * (leftspeed *(1-ratio) - k*(*disorder) + k_theta*dtheta) + leftspeed *ratio);
 
        //pc2.printf("d_length:%d disorder:%d rs:%f ls:%f daikei:%f\n\r", *d_length, *disorder, k*(*disorder) - k_theta*dtheta, -k*(*disorder) + k_theta*dtheta, daikei);
        if((direction > 0 && *d_length <= 0) || (direction < 0 &&  *d_length >= 0)) {
            move(0, 0);
            break;
        }
 
    }
 
    wait(0.2);
 
    yellow = 0;
}
 
void pmove2(int x, int y)
{
    yellow = 1;
    red=0;
    float k = 1.0;//ズレ(mm)を回転数に反映させる比例定数
    int   k_theta = 25;//ズレ(rad)を回転数に反映させる比例定数
 
    double length, d_length;
    int   disorder;
    float dtheta, ptheta;
    float daikei;
 
 
    length = sqrt(pow(x - coordinateTheta(), 2) + pow(y - coordinateTheta(), 2));
    
    pc2.printf("length:%f", length);
    
    if(length == 0) {
        red=1;
        return;
    }
 
    ptheta = giveatan(x, y);
    
    ptheta += nearPi(coordinateTheta() - ptheta);
    
    turnrad(ptheta);
 
    virtual_setup();
 
    int i = 0;
 
    while(1) {
        update();
        virtual_update();
 
        d_length = length - virtual_coordinateX();
        disorder = virtual_coordinateY();
        dtheta = virtual_coordinateTheta();
 
        if(disorder>max_disorder) {
            disorder = max_disorder;
        } else if(disorder<-max_disorder) {
            disorder = -max_disorder;
        }
 
        if(i++ < 5) {
            daikei = i/5;
        } else if(d_length < 300) {
            daikei = d_length / 300.0;
        }
        /*
                else if(absd_length > length - 30) {
                    daikei = abs(length - absd_length) / 30.0;
        */
        else
            daikei = 1;
 
        move(daikei * (rightspeed*(1-ratio) + k*disorder - k_theta*dtheta) + rightspeed*ratio,
             daikei * (leftspeed *(1-ratio) - k*disorder + k_theta*dtheta) + leftspeed *ratio);
        
        pc2.printf("length:%f, d_length:%f, vx:%d, vy:%d", length, d_length, virtual_coordinateX(), virtual_coordinateY());
        
        if(d_length <= 0) {
            move(0, 0);
            break;
        }
 
    }
 
    wait(0.2);
    
    yellow = 0;
    red = 0;
}
 
void pmove3(int x, int y)
{
    yellow = 1;
 
    float k = 1.0;//ズレ(mm)を回転数に反映させる比例定数
    int   k_theta = 25;//ズレ(rad)を回転数に反映させる比例定数
 
    int length, dx, dy;
    int *d_length, *disorder;
    int absd_length;
    float dtheta, ptheta;
    float daikei;
 
    int direction;
 
    if(abs(x - coordinateX()) > abs(y - coordinateY())) {
        direction = X_PLUS;
        length = abs(x - coordinateX());
        d_length = &dx;
        disorder = &dy;
    } else {
        direction = Y_PLUS;
        length = abs(y - coordinateY());
        d_length = &dy;
        disorder = &dx;
    }
 
    update();
    dx = x - coordinateX();
    dy = y - coordinateY();
 
    if(*d_length < 0)   //x,y減少方向なら、*d_length<0
        direction *= -1;
 
    pc2.printf("direction:%d", direction);
 
    switch(direction) {
        case X_PLUS:
            ptheta = 0;
            break;
        case Y_PLUS:
            k *= -1;
            ptheta = PI/2;
            break;
        case X_MINUS:
            k *= -1;
            ptheta = PI;
            break;
        case Y_MINUS:
            ptheta = -PI/2;
            break;
        default:
            return;
    }
 
    ptheta += nearPi(coordinateTheta() - ptheta);
 
    turnrad(ptheta);
 
    if(length == 0) return;
 
    int i = 0;
 
    while(1) {
        update_np();
        dx = x - coordinateX();
        dy = y - coordinateY();
        dtheta = coordinateTheta() - ptheta;
 
        if(*disorder>max_disorder) {
            *disorder = max_disorder;
        } else if(*disorder<-max_disorder) {
            *disorder = -max_disorder;
        }
 
        absd_length = abs(*d_length);
 
 
        if(i++ < 5) {
            daikei = i/5;
        } else if(absd_length < 300) {
            daikei = absd_length / 300.0;
        }
        /*
                else if(absd_length > length - 30) {
                    daikei = abs(length - absd_length) / 30.0;
        */
        else
            daikei = 1;
 
        move(daikei * (rightspeed*(1-ratio) + k*(*disorder) - k_theta*dtheta) + rightspeed*ratio,
             daikei * (leftspeed *(1-ratio) - k*(*disorder) + k_theta*dtheta) + leftspeed *ratio);
 
        //pc2.printf("d_length:%d disorder:%d rs:%f ls:%f daikei:%f\n\r", *d_length, *disorder, k*(*disorder) - k_theta*dtheta, -k*(*disorder) + k_theta*dtheta, daikei);
        if((direction > 0 && *d_length <= 0) || (direction < 0 &&  *d_length >= 0)) {
            move(0, 0);
            break;
        }
 
    }
 
    wait(0.2);
 
    yellow = 0;
}
 
float giveatan(int targetx,int targety)
{
    int x,y;
    float theta;
    float phi;
    update();
    x = coordinateX();
    y = coordinateY();
    theta = coordinateTheta();//自己位置取得
    
    if(targetx - x == 0) return 0; 
    
    phi = atan(double(targety - y) / double(targetx - x));//目的地への角度phi取得
    if(targetx - x < 0) 
    {
        if(targety  - y > 0)
            phi += PI;
        else if(targety - y < 0)
            phi -= PI;
    }
    
    return phi;
}
 
void back300()
{
    red = 1;
 
    float k = 0.9;
    int   k_theta = 2;
 
    int length, px, py, dx, dy;
    float daikei;
 
    update();
 
    px = coordinateX();
    py = coordinateY();
 
    length = 300;
 
    turnrad(PI + nearPi(coordinateTheta() - PI));
 
    while(1) {
        update_np();
        dx = coordinateX() - px;
        dy = coordinateY() - py;
 
        if(dy>max_disorder) {
            dy = max_disorder;
        } else if(dy<-max_disorder) {
            dy = -max_disorder;
        }
 
 
        move(-(30 + k*dy), -(32 - k*dy));
 
 
        if(dx>length) {
            move(0, 0);
            break;
        }
 
        //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
    }
 
    wait(0.2);
 
    red = 0;
}
 
 
void nxback300()
{
    red = 1;
 
    float k = 0.9;
    int   k_theta = 2;
 
    int length, px, py, dx, dy;
    float daikei;
 
    update();
 
    px = coordinateX();
    py = coordinateY();
 
    length = 300;
 
    turnrad(nearPi(coordinateTheta()));
 
    while(1) {
        update_np();
        dx = coordinateX() - px;
        dy = coordinateY() - py;
 
        if(dy>max_disorder) {
            dy = max_disorder;
        } else if(dy<-max_disorder) {
            dy = -max_disorder;
        }
 
 
        move(-(30 - k*dy), -(32 + k*dy));
 
 
        if(abs(dx)>length) {
            move(0, 0);
            break;
        }
 
        //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
    }
 
    wait(0.2);
 
    red = 0;
}
 
 
void pyback300()
{
    red = 1;
 
    float k = 0.9;
    int   k_theta = 2;
 
    int length, px, py, dx, dy;
    float daikei;
 
    update();
 
    px = coordinateX();
    py = coordinateY();
 
    length = 300;
 
    turnrad(PI/2 + nearPi(coordinateTheta() - PI/2));
 
    while(1) {
        update_np();
        dx = coordinateX() - px;
        dy = coordinateY() - py;
 
        if(dx>max_disorder) {
            dx = max_disorder;
        } else if(dx<-max_disorder) {
            dx = -max_disorder;
        }
 
 
        move(-(30 - k*dx), -(32 + k*dx));
 
 
        if(dy>length) {
            move(0, 0);
            break;
        }
 
        //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
    }
 
    wait(0.2);
 
    red
    
     = 0;
}
 
void nyback300()
{
    {
        red = 1;
 
        float k = 0.9;
        int   k_theta = 2;
 
        int length, px, py, dx, dy;
        float daikei;
 
        update();
 
        px = coordinateX();
        py = coordinateY();
 
        length = 300;
 
        turnrad(-PI/2 + nearPi(coordinateTheta() + PI/2));
 
        while(1) {
            update_np();
            dx = coordinateX() - px;
            dy = coordinateY() - py;
 
            if(dx>max_disorder) {
                dx = max_disorder;
            } else if(dx<-max_disorder) {
                dx = -max_disorder;
            }
 
 
            move(-(30 - k*dx), -(32 + k*dx));
 
 
            if(abs(dy)>length) {
                move(0, 0);
                break;
            }
 
            //pc.printf("d_length:%d disorder:%d daikei:%f\n\r", *d_length, *disorder, daikei);
        }
 
        wait(0.2);
 
        red = 0;
    }
}
 
float nearPi(float rad)
{
    float npi  = 0;
 
    while(1) {
        if(rad > npi + PI)
            npi += 2*PI;
        else if(rad < npi - PI)
            npi -= 2*PI;
        else
            return npi;
    }
}