File content as of revision 1:4b622e363fa1:

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

        Ec1.reset();
        motor1.Sc(0);
        motor2.Sc(0);
        wait(0.5);


    } else {
        motor1.Sc(-5);
        motor2.Sc(0);
        wait(-b);

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





    }


}                       //・距離とエンコーダ出力の関係      ・ジャイロの値