File content as of revision 1:651bd0514eb9:

#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);  //左
SpeedControl motor2(PA_9,PA_8,NC,1048,0.05,PB_4,PB_5);   //右

Timer timer;

int kai=0;
int n=1;
int ang=0;
int a=0;
int c=0;
int d=0;
int dis=0;
double V=0;
double v();
int e=0;
int f=0;



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

void Ang()                        //Nucleoリセット時からの機体角度(0~3600)
{
    if(spi.receive())ang=spi.read() - d;

    if(ang < 0)ang += 3600;
    if(ang > 3600)ang-=3600;

}


void print()
{
    if(kai>=500) {
        //pc.printf("count1=%d\r\n",dis);
       // pc.printf("count=%f ",motor1.getOmega());
       // pc.printf("count2=%f\r\n ",motor2.getOmega());
        //pc.printf("duty1=%f ",motor1.duty);
        //pc.printf("duty2=%f\r\n",motor2.duty);
        //pc.printf("d=%d",d);
        //pc.printf("v=%f\r\n",v());
        pc.printf("ang=%d\r\n",ang);
        kai=0;
    }
    kai++;
}


void st()
{
    motor1.stop();
    motor2.stop();
    wait(0.5);
}



void str(int a)
{

    while(n==1) {
        dis=0.301*Ec1.getCount();
        if(dis < a) {
            V=15;
            e=400;

            if(dis > a-e) {
                V = (a - dis)*V/e;
                if(V<2)V=2;
            }

            motor1.Sc(V);
            motor2.Sc(V);

        } else {
            n=2;
        }
    }
    st();
}


void back(int a)
{
    e=400;

    motor1.setDOconstant(20.7);
    motor2.setDOconstant(20.3);


    while(n==2) {
        V=15;
        dis=0.301*Ec1.getCount();
        if(dis > a) {
            if(dis < a+e ) {
                V = (a - dis)*V/e;
                if(V<2)V=2;
            }
            motor1.Sc(-V);
            motor2.Sc(-V);
        } else {
            motor1.Sc(0);
            motor2.Sc(0);
            n=1;
        }
    }
    st();
    motor1.setDOconstant(19.7);
    motor2.setDOconstant(21.3);
}


void turn(int b)
{
  c=0;
  f=700;

    while(n==2) {
        V=5;
        print();
        if(b >= 0) {


            a = ang - b;                      //角度(目標値から±1800)

            if(a < -1800) {
                a += 3600;
            } else if(a > 1800) {
                a -= 3600;
            }
            print();
            if(c==1 && a > 20) {
                motor1.Sc(-v());
                motor2.stop();
            } else if(c==2 && a < -20) {
                motor1.stop();
                motor2.Sc(-v());
            } else if(a < -f) {
                motor1.Sc(V);
                motor2.stop();
            } else if(a > f) {
                motor1.stop();
                motor2.Sc(V);
            } else if(a < -10) {
                motor1.Sc(v());
                motor2.stop();
                c=1;
            } else if(a > 10) {
                motor1.stop();
                motor2.Sc(v());
                c=2;
            } else {
                Ec1.reset();
                motor1.stop();
                motor2.stop();
                n=1;
            }

        } else {
            a = ang + b;

            if(a < -1800) {
                a += 3600;
            } else if(a > 1800) {
                a -= 3600;
            }

            if(c==1 && a > 20) {
                motor1.stop();
                motor2.Sc(v());
            } else if(c==2 && a < -20) {
                motor1.Sc(v());
                motor2.stop();
            } else if(a < -f) {
                motor1.stop();
                motor2.Sc(-V);
            } else if(a > f) {
                motor1.Sc(-V);
                motor2.stop();
            } else if(a < -10) {
                motor1.stop();
                motor2.Sc(-v());
                c=1;
            } else if(a > 10) {
                motor1.Sc(-v());
                motor2.stop();
                c=2;
            } else {
                Ec1.reset();
                motor1.stop();
                motor2.stop();
                n=1;
            }
        }
    }
    while(1){
        print();
        }
}

double v(){
    if(a < 0)a=-a;
    V=(a-10)*V/f;
    if(V < 0.3)V=0.3;
    return V;
}


int main()
{

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


    ticker.attach(&calOmega,0.05);
    ticker2.attach(&Ang,0.05);

    motor1.setPDparam(0.3,0.4);  //PDパラメータを設定
    motor2.setPDparam(0.3,0.2);  //PDパラメータを設定

    motor1.setDOconstant(19.6);
    motor2.setDOconstant(22.3);


    sw.mode(PullUp);





    if(sw==1) {                      //スタートゾーン１
        servo.pulsewidth_us(900);
        wait(0.5);
        d=ang;                       //初期角度
        n=2;
        turn(2700);
        
       /* turn(3100);
        str(950);
        turn(2410);
        str(1070);
        turn(2700);
        servo.pulsewidth_us(1800);
        str(870);
        servo.pulsewidth_us(900);
        wait(1);
        back(0);
        n=2;
        turn(-2430);
        n=2;
        back(-1080);
        n=2;
        turn(-2700);
        n=2;
        back(-970);
        n=2;
        turn(1800);
        str(500);
        servo.pulsewidth_us(1800);
        wait(0.3);

        motor1.setDOconstant(20.7);
        motor2.setDOconstant(20.3);
        while(n==2) {
            dis=0.301*Ec1.getCount();
            if(dis > 370) {
                motor1.Sc(-15);
                motor2.Sc(-15);
            } else {
                motor1.Sc(0);
                motor2.Sc(0);
                n=1;
            }
        }
        st();
        motor1.setDOconstant(19.7);
        motor2.setDOconstant(21.3);

        servo.pulsewidth_us(900);
        str(500);

        while(n==2) {
            dis=0.301*Ec1.getCount();
            if(dis > 370) {
                motor1.Sc(-15);
                motor2.Sc(-15);
            } else {
                motor1.stop();
                motor2.stop();
                n=1;
            }
        }*/

    } else {                                                  //スタートゾーン２
        servo.pulsewidth_us(1800);
        wait(0.5);
        d=ang - 1800;
        str(450);
        servo.pulsewidth_us(900);
        wait(1);
        turn(880);
        str(820);
        turn(610);
        str(1080);
        turn(1320);
        str(930);
        turn(1800);
        servo.pulsewidth_us(1800);
        n=2;
        wait(0.3);
        
        motor1.setDOconstant(20.7);
        motor2.setDOconstant(20.3);
        while(n==2) {
            dis=0.301*Ec1.getCount();
            if(dis > -130) {
                motor1.Sc(-15);
                motor2.Sc(-15);
            } else {
                motor1.stop();
                motor2.stop();
                n=1;
            }
        }

    }

}