File content as of revision 10:7c73e1577742:

#include "main.h"

/* 型定義 --------------------------------------------------------------------*/
//ゾーン対応
#define _RED_ZONE_
#ifdef  _RED_ZONE_
const int def_val=0;
#else
const int def_val=180;
#endif

/* 関数宣言 ------------------------------------------------------------------*/

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

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

//タオル掴む用
//pull: 奥のリミットで引く
//spull: 中央のリミットでつかむ
//grab: 掴む
//back_f: 真ん中目で下がるとき
int pull=0, pull_f=0, grab=0, grab_f=0, spull=0, spull_f=0;
int back_f=0;

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

//yaw: 実際に使う角度
double yaw=def_val;

//[0]: roll
//[1]: pitch
//[2]: yaw
double rawyaw[2];

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

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

//足回りのpwm値
int duty[4];

// 足回り
int lx,ly,rx;

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

int as=0;

/*----------------------------------- main -----------------------------------*/
int main()
{
    bno.reset();

    while(1) {
        //ゾーン対応
#ifdef  _RED_ZONE_
        PALETTE(RED);
#else
        PALETTE(BLUE);
#endif

        bno.setmode(OPERATION_MODE_IMUPLUS);

        //自動系のタイマースタート
        tim.start();
        drift_tim.start();

        //角度の取得
        bno.get_angles();
        rawyaw[1]=rawyaw[0];
        rawyaw[0]=bno.euler.yaw;
        
        //180～-180をまたいだ時
        if(rawyaw[1]<90 && rawyaw[0]>270) rawyaw[1]+=360;
        else if(rawyaw[1]>270 && rawyaw[0]<90) rawyaw[1]-=360;
        
        yaw-=rawyaw[0]-rawyaw[1];

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

            dif_val = yaw-Turn_val;
            //制限
            if(dif_val>=30) dif_val=30;
            else if(dif_val<=-30) dif_val=-30;

            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)*60/64;

            //R1で停止
            if(sbdbt.R1==0) {
                //メカナム基本動作
                mecanum_Move(lx, ly, rx);

                //旋回している間タイマーをリセット
                if(sbdbt.RX != 64) drift_tim.reset();

                //旋回して慣性で動いた後の角度に補正する
                if(drift_tim.read_ms()<500) Turn_val=yaw;

                //角度補正
                AngleCorrection(yaw, Turn_val);

                /*---ハンガーかけるやつ----------------------------------------------*/
                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,100);
                } else if(UPkey && limit_up == 0) {
                    as=0;
                    MD_SET_DRIVE(MD_Data, 5,MD_REVERSE);
                    MD_SET_PWM(MD_Data, 5,100);
                } 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,100);
                } else {
                    MD_SET_PWM(MD_Data,5,0);
                    MD_SET_DRIVE(MD_Data, 5,MD_BRAKE);
                }

                //タオルつかむ 四角
                if(grab_f) {
                    PALETTE(GREEN);

                    //真ん中で止まる用
                    short_f=1;
                    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,60);

                            if(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()>200) {
                                grab++;
                                tim.reset();
                            }
                            break;

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

                        case 3:
                            //腕上げる
                            i2c.Out_Set(Out_Data,0,0);
                            if(tim.read_ms()>0) {
                                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;
                                tim.reset();
                            }
                            break;
                    }
                } //if(grab_f)

                //シーツ用
                else if(spull_f) {
                    PALETTE(WHITE);
                    short_f=1;
                    switch(spull) {
                        //腕を前に動かす
                        case 0:
                            //真ん中のリミットより前に腕があった場合にバックする
                            if(back_f) MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
                            else MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
                            MD_SET_PWM  (MD_Data,4,60);

                            if(short_lim==1) {
                                spull++;
                                tim.reset();
                            } else if(max_lim) back_f=1;
                            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()>200) {
                                spull++;
                                tim.reset();
                            }
                            break;

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

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

                        //腕戻す
                        case 4:
                            MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
                            MD_SET_PWM  (MD_Data,4,60);
                            if(start_lim==1) {
                                spull_f=0;
                                tim.reset();
                            }
                            break;

                    }
                } //if(spull_f)

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

                        //停止
                        case 2:
                            if(tim.read_ms()>0) {
                                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;
                                tim.reset();
                            }
                            break;
                    }
                } //if(pull_f)

                //かごを倒す
                else if(kago_f) {
                    PALETTE(MAGENTA);
                    short_f=1;
                    //腕の先を閉じておく
                    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,100);
                            if(slide_stop==1) {
                                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()>0) {
                                kago++;
                                tim.reset();
                            }
                            break;

                        case 3:
                            MD_SET_DRIVE(MD_Data,6,MD_REVERSE);
                            MD_SET_PWM  (MD_Data,6,100);
                            if(slide_start==1) 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;
                                tim.reset();
                            }
                            break;
                    }
                } else {
                    //爪開く
                    i2c.Out_Set(Out_Data,1,1);
                    //腕上げる
                    i2c.Out_Set(Out_Data,0,0);

                    //腕真ん中で停止
                    if(short_f) {
                        if(short_lim==1) {
                            short_f=0;
                            back_f=0;
                        }
                        //奥のリミットに当たったら逆回転
                        if(max_lim==1) back_f=1;
                        //手前のリミットに当たったら正回転
                        else if(start_lim==1) back_f=0;

                        if(back_f) MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
                        else MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
                        MD_SET_PWM  (MD_Data,4,30);
                    } 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);

                    kago=0;
                    pull=0;
                    grab=0;
                    spull=0;
                    tim.reset();
                }
            }
            //非常停止
            else {
                for(int i=0; i<8; i++) {
                    MD_SET_DRIVE(MD_Data,i,MD_BRAKE);
                    MD_SET_PWM  (MD_Data,i,0);
                }
            }

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

            //モータ出力
            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("%.1f %.1f ",rawyaw[0],rawyaw[1]);
            pc.printf("yaw:%.1f dif:%.1f",yaw,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) {
                yaw=def_val;
                Turn_val=def_val;
            }
            //かごを倒す
            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) {
                    spull_f++;
                    spull_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(int lx, int ly, int rx)
{
    duty[0]=lx+ly;
    duty[1]=-(-lx+ly);
    duty[2]=-lx+ly;
    duty[3]=-(lx+ly);

    for(int i=0; i<4; i++) {
        //旋回
        duty[i]+=rx;

        //制限
        if(duty[i]>=99) duty[i]=99;
        else if(duty[i]<=-99) duty[i]=-99;
        MD_SET_DRIVE(MD_Data, i, duty[i]==0? MD_BRAKE: (duty[i]>0? MD_FORWARD: MD_REVERSE));
        MD_SET_PWM(MD_Data, i, abs(duty[i]));
    }
}

void AngleCorrection(double n_angle, double t_angle)
{
    double dif=-(t_angle-n_angle);

    if(dif>=30) dif=30;
    else if(dif<=-30) dif=-30;

    for(int i=0; i<4; i++) {
        duty[i]+=dif;
        MD_SET_DRIVE(MD_Data, i, duty[i]==0? MD_BRAKE: (duty[i]>0? MD_FORWARD: MD_REVERSE));
        MD_SET_PWM(MD_Data, i, abs(duty[i]));
    }
}

/*操作法*/
/*
〇 ボタン  ==   回収（引きずる）
? ボタン  ==   籠
△ ボタン  ==   回収（シーツ兼シャツ）
□  ボタン  ==   回収（タオル）
上 ボタン  ==   ハンガーかけ機構 上昇
下 ボタン  ==   ハンガーかけ機構 下降
L1 ボタン  ==   リセット（足回り）
L2 ボタン  ==  タオル掛け
R2 ボタン  ==  タオル真ん中掴む
select     ==  ハンガー掛け 最大まで上昇
start      ==  ハンガー展開
*/