目標座標に向かう関数

            //            deg[0]=360*rev[0]+encoder[0];
            //deg[1]=360*rev[1]+encoder[1];
//            if(flag==0) {
//                //最初は計測値を代入
//                diff[0][0]=360*rev[0]+encoder[0];
//                diff[0][1]=360*rev[0]+encoder[0];
//                flag=1;
//            } else {
//                //値が変わった場合
//                if(diff[0][1]!=360*rev[0]+encoder[0]) {
//                    //値の更新
//                    diff[0][0]=diff[0][1];
//                    diff[0][1]=360*rev[0]+encoder[0];
//                }
//            }
//            //増えた場合
//            if(diff[0][1]>diff[0][0]) {
//                //0➝359
//                if(diff[0][0]==0&&diff[0][1]==359) {
//                    deg[0]-=1;
//                } else deg[0]+=diff[0][1]-diff[0][0];
//            }
//            //減った場合
//            else if(diff[0][1]<diff[0][0]) {
//                //359➝0になった場合
//                if(diff[0][0]==359&&diff[0][1]==0) {
//                    deg[0]+=1;
//                } else deg[0]+=diff[0][1]-diff[0][0];
//            } else deg[0]=0;
            //ってかこれでよくね？
//            deg[0]+=diff[0][1]-diff[0][0];
//            deg[0]=diff[0][1]-diff[0][0];

            //if(flag==0) {
//                diff[0][0]=360*rev[0]+encoder[0];
//                flag=1;
//            } else diff[0][0]=diff[0][1];
//            diff[0][1]=360*rev[0]+encoder[0];
//            if(diff[0][1]==0&&diff[0][0]==359) deg[0]++;
//            else deg[0]+=(diff[0][1]-diff[0][0]);



//                         Pc.printf("en1::%d en2::%d \n\r",360*rev[0]+encoder[0],360*rev[1]+encoder[1]);//1::x軸 右に正 y軸 前に正

//            Pc.printf("en x::%0.0f en y ::%0.0f \n\r",deg[0],deg[1]);//1::x軸 右に正 y軸 前に正



    //30以上で反応

    //if((Sbdbt.LX-64)*100/64>30||(Sbdbt.LX-64)*100/64<-30) LX=(Sbdbt.LX-64)*100/64;
//    else LX=0;
//    if((Sbdbt.LY-64)*100/64>30||(Sbdbt.LY-64)*100/64<-30) LY=(Sbdbt.LY-64)*100/64;
//    else LY=0;
//    RX=(Sbdbt.RX-64)*100/64;

void EncoderRead()
{
    int rev[ENCODER_NUM]= {0};
    static int cnt = 0;
    static int data[ENCODER_NUM];

    if (cnt % 2 == 0) {
        if (cnt == 0) {
            for(int i=0; i<ENCODER_NUM; i++) {
                Cs[i] = 0; //データの送信をスタート
                data[i] = 0;
            }
        }
        for(int i=0; i<ENCODER_NUM; i++) Clk[i] = 1; //Doに1bitはいる
    } else {
        //Doにデータが来てないので1回待つ
        if (cnt != 1) for(int i=0; i<ENCODER_NUM; i++) data[i] |= Do[i];
        if (cnt != 25) {
            for(int i=0; ENCODER_NUM; i++) {
                data[i]<<=1;
                Clk[i] = 0;
            }
        } else {
            for(int i=0; ENCODER_NUM; i++) {
                encoder[i] = data[i] * 360 / 4096; //最大値が4096だから
                Cs[i] = 1; //データの送信を一区切り
                Pc.printf(" 1::%d   2::%d \n\r",360*rev[0]+encoder[0],360*rev[1]+encoder[1]);
            }
        }
    }
    for (int i=0; i<ENCODER_NUM; i++) {
        if(encoder[i]==0) rev[i]++;
    }
    cnt++;
    cnt %=26;
}






モーターの動かし方
for(int i=0; i<2; i++) {
        if(power[i]>0) MD_SET_DRIVE(MD_Data,i,MD_FORWARD);
        else if(power[i]<0) MD_SET_DRIVE(MD_Data,i,MD_REVERSE);
        else MD_SET_DRIVE(MD_Data,i,MD_BRAKE);
        MD_SET_PWM(MD_Data,i,abs(power[i]));
        i2c.MD_I2C(MD_Data,i);
}
    
//リミットスイッチでモーター制御

        if(Switch[0]==1) {
            Pc.printf(" left \n\r");
            MD_SET_DRIVE(MD_Data,0,MD_FORWARD);
            MD_SET_PWM(MD_Data,0,50);
        } else if(Switch[1]==1) {
            Pc.printf(" right \n\r");
            MD_SET_DRIVE(MD_Data,0,MD_REVERSE);
            MD_SET_PWM(MD_Data,0,50);
        } else {
            MD_SET_DRIVE(MD_Data,0,MD_BRAKE);
            MD_SET_PWM(MD_Data,0,0);
        }
        I2c.MD_I2C(MD_Data,0);


//
//void EncoderFlip()
//{
//    static double enc[ENCODER_NUM][2];
//    static double diff_deg[ENCODER_NUM]; //角度
//    static double diff_dis[ENCODER_NUM]; //距離
//
//    static bool flag = 0;
//
//    //値の更新があったとき
//    if (EncoderRead() == 1) {
//
//
//        for (int i = 0; i < ENCODER_NUM; i++) {
//
//            enc[i][1] = enc[i][0];
//            enc[i][0] = encoder[i];
//
//        }
//
//        if (flag) { //一回目差分を計算しない
//
//            for (int i = 0; i < ENCODER_NUM; i++) diff_deg[i] = enc[i][0] - enc[i][1];
//
//            diff_dis[LEGX_DEG] = DEG_TO_DIS(diff_deg[LEGX_DEG]);
//        } else flag = 1;
//    }
//}

//
//int EncoderRead()
//{
//    static int cnt = 0;
//    static int data[ENCODER_NUM];
//    //Pc.printf(" %d \n\r",cnt);
//    if (cnt % 2 == 0) {
//        if (cnt == 0) {
//            for (int i = 0; i < ENCODER_NUM; i++) {
//                Cs[i] = 0; //データの送信をスタート
//                data[i] = 0;
//            }
//        }
//        for (int i = 0; i < ENCODER_NUM; i++) Clk[i] = 1; //Doに1bitはいる
//    } else {
//        //Doにデータが来てないので1回待つ
//        if (cnt != 1) {
//            for (int i = 0; i < ENCODER_NUM; i++) {
//                data[i] |= Do[i];
//            }
//        }
//        if (cnt != 25) {
//            for (int i = 0; i < ENCODER_NUM; i++) {
//                data[i]<<=1;
//                Clk[i] = 0;
//            }
//        } else {
//            for (int i = 0; i < ENCODER_NUM; i++) {
//                encoder[i] = data[i] * 360 / 4096; //最大値が4096だから
//                Cs[i] = 1; //データの送信を一区切り
//            }
//            cnt++;
//            cnt %= 26;
//            return 1; //cntが25の時1
//        }
//    }
//    cnt++;
//    cnt %= 26;
//    return 0;
//
//}
//void EncoderFlip()
//{
//    static double enc[ENCODER_NUM][2];
//    static double diff_deg[ENCODER_NUM]; //角度
//    static double diff_dis[ENCODER_NUM]; //距離
//
//    static bool flag = 0;
//
//    //値の更新があったとき
//    if (EncoderRead() == 1) {
//
//
//        for (int i = 0; i < ENCODER_NUM; i++) {
//
//            enc[i][1] = enc[i][0];
//            enc[i][0] = encoder[i];
//
//        }
//
//        if (flag) { //一回目差分を計算しない
//
//            for (int i = 0; i < ENCODER_NUM; i++) diff_deg[i] = enc[i][0] - enc[i][1];
//
//            diff_dis[LEGX_DEG] = DEG_TO_DIS(diff_deg[LEGX_DEG]);
//        } else flag = 1;
//
//
//    }
//}
