File content as of revision 7:2b63f0a1b679:

#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);

DigitalIn teamSW(PC_11);
DigitalOut teamledblue(PC_10);
DigitalOut teamledred(PC_12);

DigitalIn phase1(PA_5);
DigitalIn phase2(PA_6);

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.88;

int phaseSW(void){
    
    
    phase1.mode(PullUp);
    phase2.mode(PullUp);
    int SW=phase1+2*phase2;
    pc.printf("%d\n\r",SW);
    return SW;
}
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;
        }
    }
}

void turnccw80()
{   float GODRAD=1.2;
    float pt;
    move(turnspeed,(-1)*turnspeed);

    update();
    pt=coordinateTheta();

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

void turncw80()
{   float GODRAD=1.2;
    float pt;
    move((-1)*turnspeed,turnspeed);

    update();
    pt=coordinateTheta();

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

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 teamLED()
{
    teamSW.mode(PullUp);
    if(teamSW) {
        teamledblue=1;
        teamledred=0;
        return -1;
    } else {
        teamledblue=0;
        teamledred=1;
        return 1;
    }
}

int main(){
    int team=1,phase=0;
    setup();
    initmotor();
    team=teamLED();
    phase=phaseSW();
    
    if(team>0){
        
        if(phase==0){
            movelength(600);
            turnccw();
            movelength(900);
            turnccw80();
            movelength(600-50);
            wait(30);
        }
        
        if(phase==1){
            movelength(900);
            turnccw();
            movelength(900);
            turnccw80();
            movelength(900-50);
            wait(30);
        }

        if(phase==2){
            movelength(1200);
            turnccw();
            movelength(1200);
            turnccw80();
            movelength(1200-50);
            wait(30);
        }
    }

    if(team<0){
        
        if(phase==0){
            movelength(600);
            turncw();
            movelength(900);
            turncw();
            movelength(600);
            wait(30);
        }
        
        if(phase==1){
            movelength(900);
            turncw();
            movelength(900);
            turncw();
            movelength(900);
            wait(30);   
    }
    
        if(phase==2){
            movelength(1200);
            turncw();
            movelength(600);
            turncw();
            movelength(1200);
            wait(30);
        }
    }
}