File content as of revision 1:199c4a71da88:

/*今日 やること*/

/**********************************************************************************************************************************************************************/
#include "main.h"
//#include "gyro.h"
#include <math.h>

/*******************************************************************************************************************************************************************/
#define CIRCLE sbdbt.CIRCLE
#define SQUARE sbdbt.SQUARE
#define CROSS  sbdbt.CROSS
#define TRIANGLE  sbdbt.TRIANGLE
#define LEFTkey  sbdbt.LEFTkey
#define RIGHTkey  sbdbt.RIGHTkey
#define UPkey  sbdbt.UPkey
#define DOWNkey  sbdbt.DOWNkey

/**********************************************************************************************************************************************************************/

/* 型定義 --------------------------------------------------------------------*/

MD_I2C_Data_TypeDef MD_Data[Motor_MAX] = {
    { 8, 0, 0, 0, {0}},     //足回り
    { 9, 1, 0, 0, {0}},
    {10, 0, 0, 0, {0}},
    {11, 1, 0, 0, {0}},
    {12, 0, 0, 0, {0}},     // 洗濯物回収
    {13, 1, 0, 0, {0}},     // 上下機構
    {14, 0, 0, 0, {0}},     // 洗濯物排出
    {15, 1, 0, 0, {0}}      // 左右移動
};

IN_I2C_Data_TypeDef In_Data[4] = {
    {1,0,0},        // 洗濯物排出(初期位置)
    {1,1,0},        // 洗濯物排出(停止)
    {1,2,0},        // 腕(最大  タオル)
    {1,3,0}         // 腕(初期位置)
};

IN_I2C_Data_TypeDef In_Data2[] = {
    {3,0,0},        // 上下機構(上)
    {3,1,0},        //        (下)
    {3,2,0},         // 腕(シーツ兼シャツ）

};

OUT_I2C_Data_TypeDef Out_Data[] = {
    {0,0,0},        // 腕の上下
    {0,1,0},        // 洗濯物排出
    {0,2,0},        // バスタオルの先
    {0,3,0},        // 展開
    {1,0,0},        // バスタオルの中央
};
/* 関数宣言 ------------------------------------------------------------------*/
void flip1(void);
void mecanum_Move(void);
void mecanum_Turn(void);
void mecanum_TurnMove(void);
void mecanum_Stop(void);
//int sum_deg(double deg,double old);
void big_sort(int16_t data[], uint8_t num);
int median_filter(int16_t data[], int16_t add_data, const uint8_t num);

/* 変数宣言 ------------------------------------------------------------------*/
//かご回転用
int kago=0, kago_flag=0;

//コントローラーの状態を保存するため
int once=0;

//タオル引く用
int pull=0, pull_flag=0;

//タオル掴むよう
int grab=0, grab_flag=0;


int limit_up,limit_down;            // リミットスイッチ
int slide_start,slide_stop;     //かごのリミット
int short_lim,max_lim,start_lim;


int lx,ly,rx,L,R;        // 足回り
int L_1,R_1;
int g;
double y_deg,yaw,offset;

int cross_f;
int circle_f=0,circle_cnt=0;
int triangle_f=0,triangle_cnt=0;
int sel_f = 0,sel_cnt = 0;
int left_f=0,right_f=0;
int start_s=0,start_cnt=0,start_f=0;

int val4;
int as=0;
int slide_f,slide_st;   // 機構
int back_f=0,start = 0,get=0,www=0;
int time_f=0,time_cnt=0;
int link_up=0,link_down=0;
int tach_f,back_cnt,del;
int tim_start=0;
double Turn_val,dif_val;
Timer tim;

/*クラス宣言-------------------------------------------------------------------*/

Ticker flipper1;                 // 割込み
Y_I2C i2c(PB_9, PB_8);           // I2C
DigitalOut myled(LED1);          // オンボードLED
SBDBT sbdbt(PA_0, PA_1, 9600);   // SDBDT
MyMPU6050 mpu(PC_9, PA_8);       // ジャイロセンサ

DigitalOut blue(PA_10);
DigitalOut green(PB_4);
DigitalOut red(PB_5);

/*----------------------------------- main -----------------------------------*/
/* @brief  メインプログラム
* @param  なし
* @retval なし
*/
int main()
{
    //flipper1.attach(&flip1, 0.001);    // 割り込み
    while(1) {
        //自動系のタイマースタート
        tim.start();
        mpu.loop();
        if(sbdbt.State_check()) {

            i2c.In(In_Data,0);
            i2c.In(In_Data,1);
            i2c.In(In_Data,2);
            i2c.In(In_Data,3);
            i2c.In(In_Data2,0);
            i2c.In(In_Data2,1);
            i2c.In(In_Data2,2);

            y_deg = (double)mpu.ypr[0] * 180 / M_PI;


            val4=tim.read_ms();
            slide_start  =  (In_Data+3)->in_data;
            slide_stop   =  (In_Data+0)->in_data;


            lx = ((64-sbdbt.LY)*100.0/64)*sin(yaw*M_PI/180)+((sbdbt.LX-64)*100.0/64)*cos(yaw*M_PI/180);
            ly = ((64-sbdbt.LY)*100.0/64)*cos(yaw*M_PI/180)-((sbdbt.LX-64)*100.0/64)*sin(yaw*M_PI/180);
            rx = (sbdbt.RX - 64)*100/64;
            L = lx+ly;
            R = ly-lx;

            if(sbdbt.L1) {
                Turn_val=y_deg;
                offset=y_deg;
            }

            if(LEFTkey) {
                offset=y_deg;
            }
            if(RIGHTkey) {
                offset=y_deg;
                right_f=1;
            }

            yaw = -y_deg+offset;
            dif_val=y_deg-Turn_val;

            if(sbdbt.LX !=64 || sbdbt.LY !=64) {

                mecanum_Move();

                // 以下，ジャイロセンサーのプログラム
                if(g) {
                    if(abs(ly) > 10) {
                        if(ly>10) {
                            (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)-dif_val;
                            (MD_Data+1)->PWMVal = MD_GET_PWM(MD_Data,1)+dif_val;
                        } else {
                            (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)+dif_val;
                            (MD_Data+1)->PWMVal = MD_GET_PWM(MD_Data,1)-dif_val;
                        }
                    }

                    if(abs(lx) > 10) {
                        if(lx>10) {
                            (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)-dif_val;
                            (MD_Data+2)->PWMVal = MD_GET_PWM(MD_Data,2)+dif_val;
                        } else {
                            (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)+dif_val;
                            (MD_Data+2)->PWMVal = MD_GET_PWM(MD_Data,2)-dif_val;
                        }
                    }

                    if((abs(ly)-abs(lx)) > 0) {
                        (MD_Data+2)->PWMVal = MD_GET_PWM(MD_Data,0);
                        (MD_Data+3)->PWMVal = MD_GET_PWM(MD_Data,1);
                    } else {
                        (MD_Data+1)->PWMVal = MD_GET_PWM(MD_Data,0);
                        (MD_Data+3)->PWMVal = MD_GET_PWM(MD_Data,2);
                    }
                }     // if(g)
            }// if(LX !=64 || LY !=64)
            else if(sbdbt.RX != 64) {         // 旋回
                Turn_val=y_deg;
                mecanum_Turn();
            } else {
                mecanum_Stop();
            }
            if(left_f) {        // 90°旋回
                if(abs(yaw)<60) {
                    for(int i=0; i<4; i++) {
                        MD_SET_PWM(MD_Data, i,60);
                        MD_SET_DRIVE(MD_Data, i,MD_REVERSE);
                    }
                } else {
                    left_f=0;
                    mecanum_Stop();
                }
            }
            if(right_f) {
                if(yaw>-60) {
                    for(int i=0; i<4; i++) {
                        MD_SET_PWM(MD_Data, i,60);
                        MD_SET_DRIVE(MD_Data, i,MD_FORWARD);
                    }
                } else {
                    right_f=0;
                    mecanum_Stop();
                }
            }

            /*---ハンガーかけるやつ--------------------------------------------------------------------------------------------------------------------------------------------*/
            limit_up    = (In_Data2+0)->in_data;
            limit_down  = (In_Data2+1)->in_data;
            if(sbdbt.SELECT)as=1;//selectキー

            if(as==1 && limit_up == 0) {
                MD_SET_DRIVE(MD_Data, 5,MD_REVERSE);
                MD_SET_PWM(MD_Data, 5,50);
            } else if(UPkey && limit_up == 0) {
                as=0;
                MD_SET_DRIVE(MD_Data, 5,MD_REVERSE);
                MD_SET_PWM(MD_Data, 5,50);
            } else if(as==1 && limit_up == 1) {
                as=0;
                MD_SET_PWM(MD_Data,5,0);
                MD_SET_DRIVE(MD_Data, 5,MD_BRAKE);
            } else if(DOWNkey && limit_down == 0) {
                MD_SET_DRIVE(MD_Data, 5,MD_FORWARD);
                MD_SET_PWM(MD_Data, 5,50);
            } else {
                MD_SET_PWM(MD_Data,5,0);
                MD_SET_DRIVE(MD_Data, 5,MD_BRAKE);
            }

            max_lim  = (In_Data+1)->in_data;
            start_lim  =(In_Data+2)->in_data;
            short_lim = (In_Data2+2)->in_data;
            //-------------- 洗濯物回収 --------------//
            /*---回収機構 ～押し出し～---------------------------------------------------------------------------------------------------------------------------------------*/
            //タオルつかむ 四角
            if(grab_flag) {
                switch(grab) {
                    //腕を前に動かす
                    case 0:
                        //爪開く
                        i2c.Out_Set(Out_Data,1,1);
                        MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
                        MD_SET_PWM  (MD_Data,4,40);
                        if(max_lim==1) {
                            grab++;
                            tim.reset();
                        }
                        break;

                    case 1:
                        //腕おろす
                        i2c.Out_Set(Out_Data,0,1);
                        if(tim.read_ms()>1000) {
                            grab++;
                            tim.reset();
                        }
                        break;

                    case 2:
                        //爪閉じる
                        i2c.Out_Set(Out_Data,1,0);
                        if(tim.read_ms()>500) {
                            grab++;
                            tim.reset();
                        }
                        break;

                    case 3:
                        //腕上げる
                        i2c.Out_Set(Out_Data,0,0);
                        MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
                        MD_SET_PWM  (MD_Data,4,40);
                        if(start_lim==1)    grab_flag=0;
                        break;
                }
            } //if(grab_flag)
            //タオル引く 丸   
            else if(pull_flag) {
                switch(pull) {
                    //腕を前に動かす
                    case 0:
                        //爪閉じる
                        i2c.Out_Set(Out_Data,1,0);
                        MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
                        MD_SET_PWM  (MD_Data,4,40);
                        if(max_lim==1) {
                            pull++;
                            tim.reset();
                        }
                        break;

                    case 1:
                        //腕おろす
                        i2c.Out_Set(Out_Data,0,1);
                        if(tim.read_ms()>1000) {
                            pull++;
                            tim.reset();
                        }
                        break;

                    case 2:
                        if(tim.read_ms()>500) {
                            pull++;
                            tim.reset();
                        }
                        break;

                    case 3:
                        MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
                        MD_SET_PWM  (MD_Data,4,40);
                        if(start_lim==1) pull_flag=0;
                        break;
                }
            } //if(pull_flag)
            else if(kago_flag) {
                //腕の先を閉じておく
                i2c.Out_Set(Out_Data,1,0);

                switch(kago) {
                    //腕を前に動かす
                    case 0:
                        MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
                        MD_SET_PWM  (MD_Data,4,40);
                        if(max_lim==1) kago++;
                        break;

                    //かごを倒す
                    case 1:
                        //腕停止
                        MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
                        MD_SET_PWM  (MD_Data,4,0);

                        //かご回転
                        MD_SET_DRIVE(MD_Data,6,MD_FORWARD);
                        MD_SET_PWM  (MD_Data,6,70);
                        if(slide_stop==1) {
                            kago++;
                            MD_SET_DRIVE(MD_Data,6,MD_BRAKE);
                            MD_SET_PWM  (MD_Data,6,0);
                        }
                        tim.reset();
                        break;

                    case 2:
                        if(tim.read_ms()>1500)kago++;
                        break;

                    case 3:
                        MD_SET_DRIVE(MD_Data,6,MD_REVERSE);
                        MD_SET_PWM  (MD_Data,6,65);
                        if(slide_start==1) kago++;
                        break;

                    case 4:
                        MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
                        MD_SET_PWM  (MD_Data,4,40);
                        if(start_lim==1)    kago_flag=0;
                        break;
                }
            } else {
                MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
                MD_SET_PWM  (MD_Data,4,0);
                MD_SET_DRIVE(MD_Data,6,MD_BRAKE);
                MD_SET_PWM  (MD_Data,6,0);
                
                //爪開く
                i2c.Out_Set(Out_Data,0,0);
                //腕上げる
                i2c.Out_Set(Out_Data,1,0);
                
                kago=0;
                pull=0;
                grab=0;
            }

            /*タオル掛け*/
            if(sbdbt.L2)i2c.Out_Set(Out_Data,2,1);
            else i2c.Out_Set(Out_Data,2,0);
            /*タオルを引っ張るやつ*/
            if(sbdbt.R2)i2c.Out_Set(Out_Data,4,1);
            else i2c.Out_Set(Out_Data,4,0);
            /*展開*/
            if(sbdbt.START && start_f == 0) {
                start_cnt++;
                start_f = 1;
            } else if(sbdbt.START==0 && start_f == 1) start_f =0;

            if(start_cnt%3==1) start_s=0;
            else  start_s=1;

            if(start_s==0)i2c.Out_Set(Out_Data,3,1);
            else if(start_s==1)i2c.Out_Set(Out_Data,3,0);
            else start_cnt=2;
            //-------------- 出力 --------------//
            i2c.Out(Out_Data,5);

            i2c.MD_I2C(MD_Data,0);
            i2c.MD_I2C(MD_Data,1);
            i2c.MD_I2C(MD_Data,2);
            i2c.MD_I2C(MD_Data,3);
            i2c.MD_I2C(MD_Data,4);
            i2c.MD_I2C(MD_Data,5);
            i2c.MD_I2C(MD_Data,6);
            i2c.MD_I2C(MD_Data,7);

            pc.printf("pull %2d%2d ",pull,pull_flag);
            pc.printf("grab %2d%2d ",grab,grab_flag);
            pc.printf("kago %2d%2d ",kago,kago_flag);
            pc.printf("\n\r");
//            pc.printf("%2d%2d%2d%2d%2d%2d\n\r",UPkey,RIGHTkey,DOWNkey,LEFTkey,sbdbt.SELECT,sbdbt.START);
//                printf("y_deg%2f offset%2f yaw%2f Turn_val%2f dif_val%2f\r\n",y_deg,offset,yaw,Turn_val,dif_val);

//コントローラの状態を保存
            if(CROSS) {
                if(once==0) {
                    kago_flag++;
                    kago_flag%=2;
                    once++;
                }
            } else if(SQUARE) {
                if(once==0) {
                    grab_flag++;
                    grab_flag%=2;
                    once++;
                }
            } else if(CIRCLE) {
                if(once==0) {
                    pull_flag++;
                    pull_flag%=2;
                    once++;
                }
            } else once=0;


        }    // if(sbdbt.State_check())
    }     // while(1)
}       // int main()

/* メカナムの基本移動 */
void mecanum_Move(void)
{
    int val1,val2;

    MD_SET_DRIVE(MD_Data, 0,(L > 0) ? MD_FORWARD : MD_REVERSE);
    MD_SET_DRIVE(MD_Data, 1,(R > 0) ? MD_REVERSE : MD_FORWARD);
    MD_SET_DRIVE(MD_Data, 2,(R > 0) ? MD_FORWARD : MD_REVERSE);
    MD_SET_DRIVE(MD_Data, 3,(L > 0) ? MD_REVERSE : MD_FORWARD);

    val1 = (int)sqrt( 1.0*abs( abs(lx)*lx + abs(ly)*ly ) );
    val2 = (int)sqrt( 1.0*abs( abs(ly)*ly - abs(lx)*lx ) );

    MD_SET_PWM(MD_Data,0,val1);
    MD_SET_PWM(MD_Data,1,val2);
    MD_SET_PWM(MD_Data,2,val2);
    MD_SET_PWM(MD_Data,3,val1);


    if(MD_GET_PWM(MD_Data,0)*2 < MD_GET_PWM(MD_Data,1) ) {
        MD_SET_PWM(MD_Data, 0, 0);
        g = 0;
    } else if(  MD_GET_PWM(MD_Data,1)*2 < MD_GET_PWM(MD_Data,0) ) {
        MD_SET_PWM(MD_Data, 1, 0);
        g = 0;
    }

    // 斜めじゃないなら大きいほうにPWMをそろえて完全縦横移動
    else if( MD_GET_PWM(MD_Data,0) < MD_GET_PWM(MD_Data,1) ) {
        MD_SET_PWM(MD_Data, 0, MD_GET_PWM(MD_Data,1));
        g = 1;

    } else {
        MD_SET_PWM(MD_Data, 1, MD_GET_PWM(MD_Data,0));
        g = 1;
    }

    MD_SET_PWM(MD_Data, 2, MD_GET_PWM(MD_Data,1));
    MD_SET_PWM(MD_Data, 3, MD_GET_PWM(MD_Data,0));

}

/* メカナム旋回 */
void mecanum_Turn(void)
{
    for(int i=0; i<4; i++) {
        MD_SET_PWM(MD_Data, i,sbdbt.R1?abs(sbdbt.RX - 64)*50/64:abs(sbdbt.RX - 64)*100/64);
        MD_SET_DRIVE(MD_Data, i,(sbdbt.RX < 64)? MD_REVERSE: MD_FORWARD);
    }
}

/*メカナム旋廻移動*/
void mecanum_TurnMove(void)
{
    int val1,val2;

    MD_SET_DRIVE(MD_Data, 0,(L_1 > 0) ? MD_FORWARD : MD_REVERSE);
    MD_SET_DRIVE(MD_Data, 1,(R_1 > 0) ? MD_REVERSE : MD_FORWARD);
    MD_SET_DRIVE(MD_Data, 2,(R_1 > 0) ? MD_FORWARD : MD_REVERSE);
    MD_SET_DRIVE(MD_Data, 3,(L_1 > 0) ? MD_REVERSE : MD_FORWARD);

    val1 = (int)sqrt( 1.0*abs( abs(lx)*lx + abs(ly)*ly +rx));
    val2 = (int)sqrt( 1.0*abs( abs(ly)*ly - abs(lx)*lx +rx));

    MD_SET_PWM(MD_Data,0,val1);
    MD_SET_PWM(MD_Data,1,val2);
    MD_SET_PWM(MD_Data,2,val2);
    MD_SET_PWM(MD_Data,3,val1);
}
/* 停止 */
void mecanum_Stop(void)
{
    for(int i=0; i<4; i++) {
        MD_SET_PWM(MD_Data, i, 0);
        MD_SET_DRIVE(MD_Data, i, MD_BRAKE);
    }
}


/*割り込み*/
//    void flip1(void) {
//        static int cnt=0;
//
//        /* カウントアップ，1sループ */
//        cnt++;
//        cnt%=10000;
//
//        /* 100msの処理 */
//        if(cnt%100==0) {
//            myled = !myled;
//            if(tim_start==1) time_cnt++;
//            if(time_cnt>10 && time_f==0)  {
//                time_f=1;
//            }
//        }
//}


/*操作法*/
/*
〇 ボタン  ==   回収（引きずる）
? ボタン  ==   籠
△ ボタン  ==   回収（シーツ兼シャツ）
□  ボタン  ==   回収（タオル）
上 ボタン  ==   ハンガーかけ機構 上昇
下 ボタン  ==   ハンガーかけ機構 下降
左 ボタン  ==   90回転
右 ボタン  ==   90回転
L1 ボタン  ==   リセット（足回り）
L2 ボタン  ==  タオル掛け
R1 ボタン  ==  押しながら回転でゆっくりになる
R2 ボタン  ==  タオルひっぱる奴
select     ==  ハンガー掛け 最大まで上昇
start      ==  ハンガー展開
*/
/*その他注意点*/
/*
動かすときはジャイロの値を安定させるため15秒から20秒待つ
メカナムは振動が大きいせいかナット等が外れやすいため、定期的に確認をする（特に足回り）
あまり急発進をさせない（たまに暴走）
暴走したときはコントローラーを動かしたら治るはず
コントローラー接続が切れたらSTMの黒い奴を押す（わからないときは近くの回路班へ）
*/