File content as of revision 0:3c88c8c7b112:

#include "hcsr04.h"
#include "board.h"
#include "Serial6050.h"
#define PI 3.14159265359
#define MATO 3

CERICA cerica(FEP01);
Serial6050 mpu(PC_10, PC_11, PC_12);
RawSerial pc(USBTX, USBRX, 115200);
//I2C i2c(PB_9,PB_8);

DigitalOut debugLED(D13);
DigitalIn button(PC_13);

HCSR04 usensor(D4, D6); // 超音波センサ Trigger(DO), Echo(PWMIN)

volatile char im920Flag;//無線通信
double cm_Y,cm_X;
unsigned int dist=0;
int kyoriSiki =600;//本番は600
double matoDis[MATO+1]= {0,0,0,0}; //的との距離
//double matoDis[]= {0,150,200,160};
double angle[MATO+1] {0,0,0,0};  //動く角度
double pointX[MATO+1]= {0,0,0,0};  //的のx座標
//double pointX[]= {50,50,50,50};
double pointSum=0;
double nnmDis[MATO+1]= {0,0,0,0}; //斜めの移動距離
double rot=0;   //傾き
volatile double motor[4]= {0}; //メモリアドレスを介して入出力を行うときにvolatile
//int reach=200;   //弓の射程
int reach=250;
int encMode=0;  //0が x正 y負　1が x負 y正
int sendTiming = 0;


//無線関連-----------------------------------------------------
void debugLEDbrinkThread()
{
    while(1) {
        debugLED = !button.read();
    }
}

void im920sSend(char status_)
{
    char data[1];
    data[0] = status_;              //data you wanna send
    i2c.write(0x08 << 1,data,1);
    pc.printf("send to Arduino\r\n");
}

void commArduinoThread()
{
    while(1) {
        if(im920Flag == 1) {
            im920sSend(1);
            im920Flag = 0;
        }
    }
}
void send()
{
    //busy = 1;

    Thread LED;
    LED.start(debugLEDbrinkThread);

    Thread aaaaa;
    aaaaa.start(commArduinoThread);

    debugLED = 1;
    wait_ms(1000);
    debugLED = 0;
    wait_ms(1000);

    //pc.printf("prog start\r\n");

    while (1) {
        if(sendTiming == 1) { //user button
            im920Flag = 1;
            wait_ms(100);
            sendTiming = 0;     //いるかわからない
        }
    }
}
//--------------------------------------------------------------

void move(int spd, int dir)
{
    double adjMpu = mpu.read()*3;
    float rad = dir / 180.0 * PI;
    motor[0] = -spd * cos(rad + PI / 4)- adjMpu;
    motor[1] = -spd * cos(rad - PI / 4)- adjMpu;
    motor[2] = spd * cos(rad + PI / 4)- adjMpu;
    motor[3] = spd * cos(rad - PI / 4)- adjMpu;

    for(int i = 0; i<4 ; i++) {
        cerica.Motor(i,motor[i]);
    }
}


void encoder()
{
    while (true) {
        switch(encMode) {
            case 0:
                cm_Y = -6.0 * PI * -cerica.GetY() / (double)720.0; //単位cm
                cm_X = 6.0 * PI * -cerica.GetX() / (double)720.0;
            case 1:
                cm_Y = 6.0 * PI * -cerica.GetY() / (double)720.0; //単位cm
                cm_X = -6.0 * PI * -cerica.GetX() / (double)720.0;
        }

        if (cerica.SW(0) == true) {
            pc.printf("  X ---> %.2lf     Y ---> %.2lf\r\n", cm_X, cm_Y);
        }
    }
}

void mpuR()
{
    while(1) {
        rot=mpu.read();
        if (cerica.SW(1) == true) {
            pc.printf("rot = %lf\r\n", rot);
        }
        if (cerica.SW(2) == true) {
            pc.printf("cm:%d\r\n", dist );
        }
    }
}




void moveEnc(int spd, double dir, double distance)//移動用関数---------------------------
{
    cerica.ResetY();
    cerica.ResetX();
    pc.printf("-----------------------------\r\n");
    double rad = dir / 180.0 * PI;
    pc.printf("dir:%f  dis:%f  rad=%f\r\n",dir,distance,rad);
    /*motor[0] = -spd * cos(rad + PI / 4) + adjMpu;
    motor[1] = -spd * cos(rad - PI / 4) + adjMpu;
    motor[2] = spd * cos(rad + PI / 4) + adjMpu;
    motor[3] = spd * cos(rad - PI / 4) + adjMpu;
    for(int i = 0; i<4 ; i++) {
        cerica.Motor(i,motor[i]);
    }*/
    /*for(int i=0; i<4; i++) {
        pc.printf("motor[%d] : %f\n\r",i, motor[i]);
    }*/

    //ここからエンコーダー処理
    bool keyX = true, keyY = true;
    double load_x = distance * cos(rad); //ラジアン値にすること
    double load_y = distance * sin(rad);

    if (load_x > 0) {
        load_x = floor(load_x);//小数点切り捨て
    } else {
        load_x = ceil(load_x);//小数点切り上げ
    }
    if (load_y > 0) {
        load_y = floor(load_y);
    } else {
        load_y = ceil(load_y);
    }
    pc.printf("loadX--->%.1f   loadY--->%.1f\r\n",load_x,load_y);

    while(keyX==true||keyY==true) {
        double adjMpu = mpu.read()*3;
        motor[0] = -spd * cos(rad + PI / 4) - adjMpu;
        motor[1] = -spd * cos(rad - PI / 4) - adjMpu;
        motor[2] = spd * cos(rad + PI / 4) - adjMpu;
        motor[3] = spd * cos(rad - PI / 4) - adjMpu;

        for(int i = 0; i<4 ; i++) {
            cerica.Motor(i,motor[i]);
        }
        if(load_x==0||load_x==-0) {
            keyX=false;
        }
        if(load_y==0||load_y==-0) {
            keyY=false;
        }

        if(load_x>0) {
            if (cm_X >= load_x) {
                keyX = false;
            }
        } else {
            if(cm_X<=load_x) {
                keyX = false;
            }
        }
        if(load_y>0) {
            if (cm_Y >= load_y) {
                keyY = false;
            }
        } else {
            if(cm_Y<=load_y) {
                keyY = false;
            }
        }
    }

    /* do {//keyが両方trueのとき繰り返す
         if(load_x==0) {
             keyX=false;
         }
         if(load_y==0) {
             keyY=false;
         }

         if(load_x>0) {
             if (cm_X >= load_x) {
                 keyX = false;
                 //printf("get to load_x!\r\n");
             }
         } else {
             if(cm_X<=load_x) {
                 keyX = false;
                 //printf("get to load_x!\r\n");
             }
         }
         if(load_y>0) {
             if (cm_Y >= load_y) {
                 keyY = false;
                 //printf("get to load_y!\r\n");
             }
         } else {
             if(cm_Y<=load_y) {
                 keyY = false;
                 //printf("get to load_y!\r\n");
             }
         }
     } while (keyX == true || keyY == true);
     printf("get to GOAL!!------------\n");*/
    cerica.ResetY();
    cerica.ResetX();
}
void oto(int val)
{
    //cerica.Play(rand() % val);
    cerica.Play(val);
    wait(0.3);
    cerica.Stop();
}

void rotation(int spd,int dir)
{
    mpu.reset();
    for(int i=0; i<4; i++) {
        pc.printf("motor[%d] : %d\n\r",i, spd);
        cerica.Motor(i,spd);
    }
    pc.printf("rotating\r\n");
    while(1) {
        if(dir<=rot) {
            for(int i=0; i<4; i++) {
                pc.printf("motor[%d] : 0\n\r",i);
                //cerica.Motor(i,0);
            }
            break;
        }
    }
    mpu.reset();
    pc.printf("rotation end\r\n");
}

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

void selfShot()
{
    while(1) {
        if(!button) {
            oto(40);
            sendTiming=1;
        }
    }
}

void signal()
{
    while (true) {
        cerica.Send();
    }
}

/*void Init()
{
    int tone = 0;

    // Initialize
    for (int i = 0; i < 5; i++)
        cerica.LED(i, 1);

    boardInit();
    cerica.ResetY();
    cerica.ResetX();

    //Thread Start
    Thread move;
    move.start(signal);

    Thread mpu;
    mpu.start(mpuR);

    Thread enc;
    enc.start(encoder);

    Thread wireless;
    wireless.start(send);


    cerica.Play(tone), wait(0.1), cerica.Stop(), tone++, wait(0.1);
    printf("MPU6050 Start\r\n");


    cerica.Play(tone), wait(0.1), cerica.Stop(), tone++, wait(0.1);
    printf("MPU6050 Start\r\n");

    for (int i = 0; i < 5; i++)
        cerica.LED(i, 0);

    wait(0.2);
}*/