File content as of revision 0:1f542f8756d6:

#include "mbed.h"
#include "EC.h"

PwmOut servo(PB_7);
DigitalIn sw(PA_15);
SPISlave spi(PB_15,PB_14,PB_13,PB_12);

Ec Ec1(PB_6,PC_7,NC,1048,0.05);
 Ticker ticker;
 Ticker ticker2;
 DigitalIn button(USER_BUTTON);
 Serial pc(USBTX,USBRX);
    
SpeedControl motor1(PA_10,PB_3,NC,1048,0.05,PA_1,PA_0);  //left
SpeedControl motor2(PA_9,PA_8,NC,1048,0.05,PB_4,PB_5);   //right

                                
                                     
int n=1;


void calOmega() //角速度計算関数
{
   motor1.CalOmega();
   motor2.CalOmega();
   Ec1.CalOmega();
}

void st()
{
    int kai=0;
    while(kai<50000){
    motor1.stop();
    motor2.stop();
    kai=kai + 1;
 }
}

/*void print()
{
   // pc.printf("n=%d",n);
   // pc.printf("count1=%d",Ec1.getCount());
   // pc.printf("gyro=%d",spi.read());     
    pc.printf("count1=%f ",motor1.getOmega());
    pc.printf("count2=%f ",motor2.getOmega());
    pc.printf("duty1=%f ",motor1.duty);
    pc.printf("duty2=%f\r\n",motor2.duty);
}*/

void str(int a)      //
{
int kai=0;
 
 while(n==1){
 int dis=0.301*Ec1.getCount(); 
   if(kai>=500){       //ループ500回ごとに角速度・出力duty比を表示
   //  if(spi.receive()){
    pc.printf("count1=%f ",motor1.getOmega());
    pc.printf("count2=%f ",motor2.getOmega());
    pc.printf("duty1=%f ",motor1.duty);
    pc.printf("duty2=%f\r\n",motor2.duty);
            kai=0;
   //     }
   }
 if(dis < a){              //距離 
   motor1.Sc(5);
   motor2.Sc(5);
  }else{
   n=2;
   }
  kai++;
  //pc.printf("kai=%d\r\n",kai);
 
}
st();
}
void back(int a)
{
 int kai=0;
 while(n==2){
 int dis=0.301*Ec1.getCount();
 kai++;
  if(kai>=500){       //ループ500回ごとに角速度・出力duty比を表示
   //  if(spi.receive()){
    pc.printf("count1=%f ",motor1.getOmega());
    pc.printf("count2=%f ",motor2.getOmega());
    pc.printf("duty1=%f ",motor1.duty);
    pc.printf("duty2=%f\r\n",motor2.duty);
            kai=0;
   //     }
   }
 if(dis > a){ 
     motor1.Sc(-5);
     motor2.Sc(-5);
    }else{
     motor1.Sc(0);
     motor2.Sc(0);
     n=1;
     }
 }
 st();
}

void turnR(int b,int c)
{
  int kai=0;
 int ang=0;
 while(n==2){

      kai++;
      if(kai>=500){       //ループ500回ごとに角速度・出力duty比を表示
   //  if(spi.receive()){
   // pc.printf("count1=%f ",motor1.getOmega());
    pc.printf("count2=%f ",motor2.getOmega());
   // pc.printf("duty1=%f ",motor1.duty);
    pc.printf("duty2=%f\r\n",motor2.duty);
   // pc.printf("ang=%f\r\n",ang);
            kai=0;
   //     }
   }
 /* if(spi.receive()){
    pc.printf("count1=%d",Ec1.getCount());
    pc.printf("gyro=%d\r\n",spi.read());
    }*/
  if(spi.receive()){
     ang=spi.read();
// pc.printf("ang=%d\r\n",ang);
 }
 if((ang < b) || (ang > c)){                   //角
   motor1.Sc(5);
   motor2.Sc(-5);
   
   }else{
     Ec1.reset();
     motor1.Sc(0);
     motor2.Sc(0);
     n=1;
 }
}
st();
}

void turnL(int b,int c)
{
 int kai=0;
 int ang=0;
 while(n==2){

      kai++;
      if(kai>=500){       //ループ500回ごとに角速度・出力duty比を表示
   //  if(spi.receive()){
    pc.printf("count1=%f ",motor1.getOmega());
  //  pc.printf("count2=%f ",motor2.getOmega());
    pc.printf("duty1=%f\r\n ",motor1.duty);
  //  pc.printf("duty2=%f\r\n",motor2.duty);
   // pc.printf("ang=%f\r\n",ang);
            kai=0;
   //     }
   }
/* if(spi.receive()){
    pc.printf("n=%d",n);
    pc.printf("count1=%d",Ec1.getCount());
    pc.printf("gyro=%d\r\n",spi.read());
    }*/
 if(spi.receive()){
     ang=spi.read();
// pc.printf("ang=%d\r\n",ang);
 }
 if((ang < b) || (ang > c)){                    //角度
   motor1.Sc(-5);
   motor2.Sc(5);
   }else{
     Ec1.reset();
     motor1.Sc(0);
     motor2.Sc(0);
     n=1;
 }
 
 }
 st();
}


int main() {

    servo.period_ms(20);
    spi.format(16,3);
    spi.frequency(1000000);

    
    ticker.attach(&calOmega,0.05);
   // ticker2.attach(&print,0.5);

    motor1.setPDparam(0.55,0.5);  //PDパラメータを設定
    motor2.setPDparam(0.6,0.5);  //PDパラメータを設定
    
    motor1.setDOconstant(13.1);         
    motor2.setDOconstant(15.8);         
    
    
    sw.mode(PullUp);
    

    
  if(sw==1){                       //スタートゾーン１
    
            servo.pulsewidth_us(900);
            str(540);
            turnL(2650,2750); 
            str(1481);
            turnL(1750,1850);
            str(540);
            turnR(2650,2750); 
            str(1481);
            servo.pulsewidth_us(1800);
            wait(0.5);
            back(0);
            n=2;
            turnR(3550,3600);
            str(540);
            turnR(850,950);
            str(1481);
            turnR(1750,1850);
            str(570);
            servo.pulsewidth_us(900);
            back(450);
            servo.pulsewidth_us(1800);
            str(570);          
           
                                                            //押し出し
                                                                   
    }else{                                                   //スタートゾーン２
           servo.pulsewidth_us(900);
           str(540);
           servo.pulsewidth_us(1800);
           wait(0.5);
           turnL(850,950);
           str(1481);
           turnL(0,50);
           str(540);
           turnR(850,950);
           str(1481);
           turnR(1750,1850);
           str(570);
           servo.pulsewidth_us(900);
           back(450);
           servo.pulsewidth_us(1800);
           str(570);
            
            
            
        
    
  }
    
    
}                       //・距離とエンコーダ出力の関係      ・ジャイロの値