File content as of revision 0:a92631a4021d:

#include "mbed.h"
#include "F3rc.h"

int main()
{
    gyro.initialize();    //main関数の最初に一度だけ実行
    gyro.acc_offset();    //やってもやらなくてもいい

    printf("start\r\n");
    motor_tick.attach(&calOmega,0.05);
    motorR.setDOconstant(30);
    motorL.setDOconstant(30);//c
    motorR.setPDparam(0.1,0);
    motorL.setPDparam(0.1,0);



    EC1.setDiameter_mm(48);//sokuteirinnhannkei
    double  getDistance_mm();
    void reset  ();
    EC1.reset();

    motor_f.period_ms(30);
    motor_b.period_ms(30);//arm

    while(1) {
        angle=gyro.getAngle()*(-1);    //角度の値を受け取る
        EC1.getDistance_mm();
        if(target_R==0) motorR.stop();
        else motorR.Sc(target_R);
        if(target_L==0) motorL.stop();
        else motorL.Sc(target_L);

        new_dist=EC1.getDistance_mm();
        d_dist=new_dist-old_dist;
        old_dist=new_dist;
        double d_x=d_dist*sin(angle*3.1415926535/180);
        double d_y=d_dist*cos(angle*3.1415926535/180);
        x=x+d_x;
        y=y+d_y;

        if(kai>=3) {
            pc.printf("R=%f L=%f",target_R,target_L);
            pc.printf("omg_R=%f omg_L=%f  \r\n",motorR.getOmega(),motorL.getOmega());
            pc.printf("i=%d\r\n",i);
            pc.printf("EC1=%f",EC1.getDistance_mm(),EC1.getCount());
            pc.printf("x=%f y=%f",x,y);
            //pc.printf("angle=%f\r\n",angle);
            //機体の進む方向、右モーターの角速度、左モーターの角速度を表示
            kai=0;
        }
        kai++;
        if(i==0) {
            servo.pulsewidth_us(950);
            if(reset_f.read()==0&&button.read()==0) {
                wait(0.05);
                if(reset_f.read()==0&&button.read()==0) {
                    denjiben=1;
                    x=0;
                    y=0;
                    i++;
                }
            }
            if(reset_a.read()==0&&button.read()==0) {
                wait(0.05);
                if(reset_a.read()==0&&button.read()==0) {
                    denjiben=1;
                    x=0;
                    y=0;
                    i=21;
                }
            }//x,y
        }
        if(i==1) {
            wait(3);
            servo.pulsewidth_us(950);
            wait(1);
            i++;
        }

        if(i==2) {
            susumu_y(1,1,1000);
        }
        if(i==3) {

            motorR.stop();
            motorL.stop();
            servo.pulsewidth_us(2200);
            wait(1);
            denjiben=0;
            wait(1);
            i++;
        }
        if(i==4) {
            servo.pulsewidth_us(950);
            wait(1);
            i++;
        }
        if(i==5) {
            susumu_y(-1,-1,200);
        }
        if(i==6) {
            motorR.stop();
            motorL.stop();
            denjiben=1;
            wait(1);
            i++;
        }

        if(i==7) {
            tgt(0,0);
        }
        if(i==10) {
            t.start();
            while(t<1) {
                motor_f=0.82;
                motor_b=0;
            }
            t.stop();
            t.reset();

            t.start();
            while(t<1) {
                motor_f=0;
                motor_b=0.82;
            }
            t.stop();
            t.reset();
            motor_f=0;
            motor_b=0;
            i++;
        }
        if(i==11) {
            susumu_y(1,1,1000);
        }
        if(i==12) {
            susumu_ang(0.5,1,90);
        }
        if(i==13){
            susumu_xl(1,1,500);
        }
        if(i==14){
            susumu_ang(0.5,1,180);
        }
        if(i==15) {
            susumu_y(1,1,415);
        }
        if(i==16) {
            susumu_ang(0.5,1,270);
        }
        if(i==17){
            susumu_xr(1,1,585);
        }
        if(i==18){
            susumu_ang(0.5,1,360);
        }
        if(i==19){
            i=11;
            }

        if(i==21) {
            susumu_y(1,1,300);
        }
        if(i==22) {
            susumu_ang(0.333,1,270);
        }
        if(i==23){
            susumu_xr(1,1,-300);
        }
        if(i==24){
            susumu_ang(1,0.333,0);
        }
        if(i==25) {
            susumu_y(1,1,300);
        }
        if(i==26) {
            susumu_ang(1,0.333,-270);
        }
        if(i==27){
            susumu_xl(1,1,300);
        }
        if(i==28){
            susumu_ang(0.333,1,0);
        }
        if(i==29){
            susumu_y(1,1,0);
            }








    }









}