File content as of revision 0:3c88c8c7b112:

#include "mbed.h"
#include "utility.h"
#define MATO 3
#define PI 3.14159
int fSpd;

void startSpd()
{
    for(fSpd=0; fSpd<=160; fSpd+=5) {
        wait_ms(15);
    }
}

void dis() //距離測定関数-----------------------------------
{
    pc.baud(115200);
    //的との位置を把握
    int a = 3;
    while(matoDis[1]==0 || matoDis[2]==0 || matoDis[3]==0) {
        while(1) {
            move(fSpd,180);    //ひだり
            usensor.start();
            dist = usensor.get_dist_cm();
            wait_ms(10);
            /*for(int i=0; i<4; i++) {
                pc.printf("motor[%d] : %f   ",i, motor[i]);
            }
            pc.printf("\r\n");*/

            if(dist < kyoriSiki && dist != 0) {
                matoDis[a] = dist;//的との距離
                pointX[a] = -cm_X - pointSum;//的がある場所のX方向の距離
                pointSum += pointX[a];
                oto(10);
                //  pointX[a]=-cm_X;
                //  cerica.ResetX();
                pc.printf("matoDis[%d]:%f\r\n",a,matoDis[a]);
                pc.printf("pointX[%d]:%f\r\n",a,pointX[a]);

                while(1) {
bbb:
                    move(fSpd,180);    //ひだり
                    usensor.start();
                    dist = usensor.get_dist_cm();
                    wait_ms(10);
                    if(dist>kyoriSiki) {
                        while(1) {
                            move(fSpd,180);    //ひだり
                            usensor.start();
                            dist = usensor.get_dist_cm();
                            wait_ms(10);
                            if(dist<kyoriSiki && dist!=0) {
                                goto bbb;
                            }
                            a--;
                            pc.printf("a=%d\n\r",a);
                            goto aaa;
                        }
                    }
                }
            }
        }
aaa:
        // pc.printf("matoDis[%d]:%f\r\n",a-1,matoDis[a-1]);
    }
    a=1;
    wait_ms(300);  //測り終わった後 秒進む（要調整）
    stop();
    pointX[0]=-cm_X - pointSum;//kokokokokokokokoko
    //pointX[0]=-cm_X;
    for(int i=0; i<=MATO; i++) {
        pc.printf("matoDis[%d]:%f\r\n",i,matoDis[i]);
        pc.printf("pointX[%d]:%f\r\n",i,pointX[i]);
    }
    wait_ms(1000);

    //回転
    rotation(80,170);
    stop();
    encMode=1;
    cerica.ResetX();
    cerica.ResetY();


    //角度とか
    angle[0]=atan((matoDis[1]-reach)/pointX[0]);
    for(int i=1; i<MATO; i++) {
        angle[i]=atan((matoDis[i+1]-matoDis[i])/pointX[i]);
    }
    angle[MATO]=-atan((matoDis[MATO]-reach)/pointX[MATO]);

    pc.printf("------------------------\r\n");
    for(int i=0; i<=MATO; i++) {
        pc.printf("matoDis[%d]:%f\n\r",i,matoDis[i]);
    }
    pc.printf("------------------------\r\n");


    for(int i=0; i<=MATO; i++) {
        pc.printf("pointX[%d]:%f\n\r",i,pointX[i]);
    }
    //反転のずれ直す
    for(int i=0; i<=MATO; i++) {
        angle[i]=180/PI*(angle[i]+PI);
        pc.printf("angle[%d]:%f\n\r",i,angle[i]);
    }
    pointX[0]-=85;
    pointX[3]-=70;


    //進む距離
    //pc.printf("nakami:%d\r\n",(matoDis[1]-reach)*(matoDis[1]-reach)+pointX[0]*pointX[0]);
    nnmDis[0]=sqrt((matoDis[1]-reach)*(matoDis[1]-reach)+pointX[0]*pointX[0]);
    for(int i=1; i<MATO; i++) {
        nnmDis[i]=sqrt((matoDis[i]-matoDis[i+1])*(matoDis[i]-matoDis[i+1])+pointX[i]*pointX[i]);
    }
    nnmDis[MATO]=sqrt((matoDis[MATO]-reach)*(matoDis[MATO]-reach)+pointX[MATO]*pointX[MATO]);

    for(int i=0; i<=MATO; i++) {
        // nnmDis[i]=180/PI*angle[i];
        pc.printf("nnmDis[%d]:%f\n\r",i,nnmDis[i]);
    }

}


//main------------------------------
int main()
{
    mpu.init();
    cerica.ResetY();
    cerica.ResetX();

    Thread move;
    move.start(signal);

    Thread m;
    m.start(mpuR);

    Thread enc;
    enc.start(encoder);
    
    Thread bow;
    bow.start(selfShot);

    Thread wireless;
    wireless.start(send);
    
    Thread slow;
    
    boardInit();

    oto(20);

    pc.printf("start------------------------\r\n");
    //合体
    moveEnc(100,-90,70);    //本番距離変える
    wait_ms(1500);
    pc.printf("Left move");
    //x方向に移動しながら距離測定
    slow.start(startSpd);
    dis();
    wait_ms(1000);
    //じぐざくいどう
    for(int i=0; i<=3; i++) {
        moveEnc(140,angle[i],nnmDis[i]);
        sendTiming = 1;
        stop();
        //撃つタイミング送信
        wait_ms(1000);
    }

    for(int i=0; i<4; i++) {
        pc.printf("motor[%d] : 0\n\r",i);
        cerica.Motor(i,0);
    }
    for(int i=0; i<3; i++) {
        pc.printf("matoDis[%d]:%f\r\n",i,matoDis[i]);
    }
}