File content as of revision 4:dcb03da10fa7:

//受渡前での妨害に対応したい
#include"header.h"
Serial pc(SERIAL_TX, SERIAL_RX);

void movelength(int length){
    int px,py,pt;
    update(-right.getPulses(), left.getPulses());
    px=coordinateX();
    py=coordinateY();
    pt=coordinateTheta();
    
    move(30,30);
    while(1){ 
        update(-right.getPulses(), left.getPulses());
        if((px-coordinateX())*(px-coordinateX())+(py-coordinateY())*(py-coordinateY())>length*length){break;}
        
        }
}


void turncw(int degree) {
  initencorder();
  initmotor();
  
  int rad=0,np=1;
  if(degree<0){np=-1;}
  rad=degree*PI/180;
  move(30*np,-30*np);
  float t,theta;
  int x,y;
  update(-right.getPulses(), left.getPulses());
  t=coordinateTheta();
  while(1){

      pc.printf("x:%d y:%d t:%f \n\r",x,y,theta*180/PI);
      x=coordinateX();
      y=coordinateY();
      theta=coordinateTheta();
      update(-right.getPulses(), left.getPulses());
      if(theta-t > np*rad){break;}
      if(theta-t < (-1)*np*rad){break;}
       //   moveto (30,30);
       /*
               move(30,30);
          update(-right.getPulses(), left.getPulses());
          pc.printf("x:%d y:%d t:%f \n\r",coordinateX(), coordinateY(), coordinateTheta());
          if(coordinateX()>300){
              while(1){*/
             
  }
  move(0,0);
}


int main(){
    
    movelength(600);
    turncw(90);
    movelength(1200);
    turncw(90);
    
}