File content as of revision 4:236e5c58e8ee:

/*今日 やること*/

/******************************************************************************/
#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[] = {
    {1,0,0},        // 洗濯物排出(初期位置)
    {1,1,0},        // 洗濯物排出(停止)
    {1,2,0},        // 腕(最大  タオル)
    {1,3,0},         // 腕(初期位置)
    {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 mecanum_Move(void);
void mecanum_Turn(void);
void mecanum_Stop(void);

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

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

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

//タオル掴む用
//sgrab_f: 中央のリミットでつかむ
int grab=0, grab_f=0, sgrab_f=0;

//タオル横展開用
int unfold_f=0;

//90度回転のフラグ
int left_f=0, right_f=0;

//y_deg: 起動時から絶対の角度
//yaw: 相対角 初期状態が0になるようにする
//offset: 初期状態を0にするために引く値
double y_deg, offset;
int yaw=0;

//Turn_val: 補正の目標値
//dif_val: 補正値との差
double Turn_val, dif_val;

//short_lim: 腕中央
//max_lim: 腕奥側
//short_lim: 腕手前側
int short_lim, max_lim, start_lim;

//機体の前方を変更する
int dir=0;

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

int lx,ly,rx,L,R;        // 足回り
int g;

int as=0;

/*クラス宣言-------------------------------------------------------------------*/
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);

//半自動の時に使う
Timer tim;

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

            for(int i=0; i<7; i++) i2c.In(In_Data,i);

//            y_deg = (double)mpu.ypr[0] * 180 / M_PI;
//            yaw = offset-y_deg;
            yaw=90*dir;
            yaw%=360;
            dif_val = y_deg-Turn_val;

            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.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();
            }

            // 90°旋回
            if(left_f) {
                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;
                    Turn_val=y_deg;
                    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;
                    Turn_val=y_deg;
                    mecanum_Stop();
                }
            }

            /*---ハンガーかけるやつ----------------------------------------------*/
            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);
            }

            //タオルつかむ 四角
            if(grab_f || sgrab_f) {
                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((sgrab_f && short_lim) || max_lim==1) {
                            grab++;
                            tim.reset();
                        }
                        break;

                    case 1:
                        //腕停止
                        MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
                        MD_SET_PWM(MD_Data,4,0);

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

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

                    case 3:
                        //腕上げる
                        i2c.Out_Set(Out_Data,0,0);
                        if(tim.read_ms()>300) {
                            grab++;
                            tim.reset();
                        }
                        break;

                    case 4:
                        MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
                        MD_SET_PWM  (MD_Data,4,60);
                        if(start_lim==1) {
                            grab_f=0;
                            sgrab_f=0;
                        }
                        break;
                }
            } //if(grab_f)

            //タオル引く 丸
            else if(pull_f) {
                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,60);
                        if(max_lim==1) {
                            pull++;
                            tim.reset();
                        }
                        break;

                    case 1:
                        //腕停止
                        MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
                        MD_SET_PWM  (MD_Data,4,0);

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

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

                    case 3:
                        MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
                        MD_SET_PWM  (MD_Data,4,60);
                        if(start_lim==1) pull_f=0;
                        break;
                }
            } //if(pull_f)

            //かごを倒す
            else if(kago_f) {
                //腕の先を閉じておく
                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,60);
                        if(max_lim==1) {
                            kago++;
                            tim.reset();
                        }
                        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,80);
                        if(slide_stop==1 || tim.read_ms()>3000) {
                            kago++;
                            tim.reset();
                            MD_SET_DRIVE(MD_Data,6,MD_BRAKE);
                            MD_SET_PWM  (MD_Data,6,0);
                        }
                        break;

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

                    case 3:
                        MD_SET_DRIVE(MD_Data,6,MD_REVERSE);
                        MD_SET_PWM  (MD_Data,6,80);
                        if(slide_start==1 || tim.read_ms()>3000) kago++;
                        break;

                    case 4:
                        MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
                        MD_SET_PWM  (MD_Data,4,60);
                        if(start_lim==1)    kago_f=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,1,1);
                //腕上げる
                i2c.Out_Set(Out_Data,0,0);

                kago=0;
                pull=0;
                grab=0;

                tim.reset();
            }

            //-------------- 出力 --------------//
            //第二引数には使う最大の個数
            i2c.Out(Out_Data,5);

            //モータ出力
            for(int i=0; i<8; i++) i2c.MD_I2C(MD_Data,i);

            /* ----------- りみっと --------------- */
            limit_up    = (In_Data+4)->in_data;
            limit_down  = (In_Data+5)->in_data;
            max_lim  = (In_Data+1)->in_data;
            start_lim  =(In_Data+2)->in_data;
            short_lim = (In_Data+6)->in_data;
            slide_start  =  (In_Data+3)->in_data;
            slide_stop   =  (In_Data+0)->in_data;

            //半自動のデバック
//            pc.printf("pull %2d%2d ",pull,pull_f);
//            pc.printf("grab %2d%2d ",grab,grab_f);
//            pc.printf("kago %2d%2d ",kago,kago_f);
//            pc.printf("deg:%f dif:%f ",y_deg,dif_val);
//            pc.printf("%2d ",dir);
            pc.printf("\n\r");


            /* --------------電磁弁系-------------------- */
            /*タオル掛け*/
            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(unfold_f)i2c.Out_Set(Out_Data,3,1);
            else i2c.Out_Set(Out_Data,3,0);


            /* ----------------ボタン系--------------- */
            //補正角リセット
            if(sbdbt.L1) {
                Turn_val=y_deg;
                offset=y_deg;
            }
            //90度回転して補正の角度を固定する
            if(LEFTkey) {
                if(once==0) {
                    dir++;
//                    left_f++;
//                    left_f%=2;
                    once++;
                }
            } else if(RIGHTkey) {
                if(once==0) {
                    dir--;
//                    right_f++;
//                    right_f%=2;
                    once++;
                }
            }
            //かごを倒す
            else if(CROSS) {
                if(once==0) {
                    kago_f++;
                    kago_f%=2;
                    once++;
                }
            }
            //タオルを掴んで動かす
            else if(SQUARE) {
                if(once==0) {
                    grab_f++;
                    grab_f%=2;
                    once++;
                }
            } else if(TRIANGLE) {
                if(once==0) {
                    sgrab_f++;
                    sgrab_f%=2;
                    once++;
                }
            }
            //倒すを引いて動かす
            else if(CIRCLE) {
                if(once==0) {
                    pull_f++;
                    pull_f%=2;
                    once++;
                }
            }
            //タオル展開用
            else if(sbdbt.START) {
                if(once==0) {
                    unfold_f++;
                    unfold_f%=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_Stop(void)
{
    for(int i=0; i<4; i++) {
        MD_SET_PWM(MD_Data, i, 0);
        MD_SET_DRIVE(MD_Data, i, MD_BRAKE);
    }
}

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