File content as of revision 0:2694d5a2f90a:

#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 dis=0;  
int ang=0;                             
                                     
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;
 }
}

int Dis(){
    dis=0.301*Ec1.getCount(); 
    return dis;
    }
    


/*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){
   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("gyro=%d",spi.read());
            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){
 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(-4.9);
     motor2.Sc(-5.2);
    }else{
     motor1.Sc(0);
     motor2.Sc(0);
     n=1;
     }
 }
 st();
}

void turnR(double b)
{
 // int kai=0;
 //int ang=0;


 if(b > 0){                   //角
   motor1.Sc(5);
   motor2.Sc(0);
   wait(b);
   
   Ec1.reset();
   motor1.Sc(0);
   motor2.Sc(0);
   wait(0.5);
   
   }else{
     motor1.Sc(0);
     motor2.Sc(-5);
     wait(-b);
     
     Ec1.reset();
     motor1.Sc(0);
     motor2.Sc(0);
     wait(0.5);
     
}
n=1;
}

void turnL(double b)
{
 // int kai=0;
 //int ang=0;


 if(b > 0){                   //角
   motor1.Sc(0);
   motor2.Sc(5);
   wait(0.815);
   
   Ec1.reset();
   motor1.Sc(0);
   motor2.Sc(0);
   wait(0.5);
   
   
   }else{
     motor1.Sc(-5);
     motor2.Sc(0);
     wait(0.81);
     
     Ec1.reset();
     motor1.Sc(0);
     motor2.Sc(0);
     wait(0.5);
     
}
n=1;
}

/*void turn(){
     motor1.Sc(-5);
     motor2.Sc(5);
     wait(1.5);
     
    
    }*/


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.3,0.4);  //PDパラメータを設定
    motor2.setPDparam(0.35,0.4);  //PDパラメータを設定
    
    motor1.setDOconstant(14.4);         //duty比と角速度の変換係数を46.3に設定←設定必要かも
    motor2.setDOconstant(15.8);        
    
    
    sw.mode(PullUp);
    

    
  if(sw==1){                       //スタートゾーン１
    
            servo.pulsewidth_us(900);
            str(395);
            turnL(0.8); 
            str(1151);
            turnL(0.8);
            str(310);
            turnR(0.74);
            servo.pulsewidth_us(1800);
            str(1326);
            servo.pulsewidth_us(900); 
            wait(0.5);
            back(0);
            turnR(-0.84);
            str(340);
            turnR(0.8);
            str(1131);
            turnR(0.8);
            str(375);
            servo.pulsewidth_us(1800);
            back(245);
            servo.pulsewidth_us(900);
            str(365);          
                    
           
                                                            //押し出し
                                                                   
    }else{                                                   //スタートゾーン２
           servo.pulsewidth_us(1800);
           str(400);
           servo.pulsewidth_us(900);
           wait(0.5);                                           //
           turnL(0.8);
           str(1131);
           turnL(0.81);
           str(270);
           turnR(0.77);
            str(1131);
            turnR(0.77);
            str(380);
            servo.pulsewidth_us(1800);
            back(260);
            servo.pulsewidth_us(900);
            str(390);
            
            
            
        
    
  }
    
    
}                       //・距離とエンコーダ出力の関係      ・ジャイロの値