File content as of revision 20:3f68738c4019:

#include "main.h"

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

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

/* 変数宣言 ------------------------------------------------------------------*/

//0: 赤
//1: 青
int zone=0;

//デフォルトの角度
int def_val=0;

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

//kogo: かごをひっくり返す
//pull: 奥のリミットで引く
//spull: 中央のリミットでつかむ(short_pull
//grab: 掴む
//back_f: 真ん中目で下がるとき
//release_f: 全体のエアー開放用
//unfold_f: タオル横展開用

int kago=0, kago_f=0, pull=0, pull_f=0, grab=0, grab_f=0, spull=0, spull_f=0;
int back_f=0, release_f=0, unfold_f=0, short_f=0;

//妨害に使う
int disturb, disturb_f;

//上下動作を保持
//[0]: 上
//[1]: 下
int keep_f[2]= {0};

//実際に使う角度
double yaw;

//[0]: 今回の値
//[1]: 前回の値
double rawyaw[2];

//Turn_val: 補正の目標値
double Turn_val;

//short_lim: 腕中央
//max_lim: 腕奥側
//short_lim: 腕手前側
int short_lim, max_lim, start_lim;
int limit_up,limit_down;        // リミットスイッチ
int slide_start,slide_stop;     //かごのリミット

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

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

int rod_hit=0;

/*----------------------------------- main -----------------------------------*/
int main()
{
    //リミット
    i2c.In(In_Data,7);
    zone=(In_Data+7)->in_data;

    if(zone==0) def_val=0;
    else def_val=180;

    yaw=def_val;

    //補正角度
    Turn_val=def_val;

    sbdbt.LX=64;
    sbdbt.LY=64;
    sbdbt.RX=64;
    sbdbt.RY=64;
    //ジャイロリセット
    bno.reset();

    while(1) {
        //自動系のタイマースタート
        tim.start();
        drift_tim.start();
        keep_tim[0].start();
        keep_tim[1].start();

        bno.setmode(OPERATION_MODE_IMUPLUS);
        //角度の取得
        bno.get_angles();
        rawyaw[1]=rawyaw[0];
        rawyaw[0]=bno.euler.yaw;

        if(rawyaw[0]!=0) {
            //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];
            yaw=fmod(yaw, 360.0);
        }

        //ゾーン対応
        if(zone==0) PALETTE(RED);
        else PALETTE(BLUE);

        //リミット
        for(int i=0; i<9; i++) i2c.In(In_Data,i);

        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(1) {
            //メカナム基本動作
            mecanum_Move(lx, ly, rx);

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

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

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

            /*---ハンガーかけるやつ----------------------------------------------*/
            //上方向
            if(!DOWNkey && ((UPkey || keep_f[0]) && limit_up == 0)) {
                //一定秒以上押されたら保持
                if(keep_tim[0].read_ms()>500) keep_f[0]=1;
                //下方向の保持を解除
                keep_f[1]=0;
                keep_tim[1].reset();

                MD_SET_DRIVE(MD_Data, 5,MD_FORWARD);
                MD_SET_PWM(MD_Data, 5,100);
            }
            //下方向
            else if(!UPkey && ((DOWNkey || keep_f[1]) && limit_down == 0)) {
                if(keep_tim[1].read_ms()>500) keep_f[1]=1;
                keep_f[0]=0;
                keep_tim[0].reset();

                MD_SET_DRIVE(MD_Data, 5,MD_REVERSE);
                MD_SET_PWM(MD_Data, 5,100);
            } else {
                keep_f[0]=0;
                keep_tim[0].reset();
                keep_f[1]=0;
                keep_tim[1].reset();

                MD_SET_PWM(MD_Data,5,0);
                MD_SET_DRIVE(MD_Data, 5,MD_BRAKE);
            }

            //タオルつかむ 四角
            if(grab_f) {
                //真ん中で止まる用
                short_f=1;
                switch(grab) {
                    //腕を前に動かす
                    case 0:
                        //爪開く
                        i2c.Out_Set(Out_Data,1,1);
                        //腕上げる
                        i2c.Out_Set(Out_Data,0,0);

                        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) {
                short_f=1;
                switch(spull) {
                    //腕を前に動かす
                    case 0:
                        //爪閉じる
                        i2c.Out_Set(Out_Data,1,0);
                        //腕上げる
                        i2c.Out_Set(Out_Data,0,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);

                        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) {
                short_f=1;
                switch(pull) {
                    //腕を前に動かす
                    case 0:
                        //爪閉じる
                        i2c.Out_Set(Out_Data,1,0);
                        //腕上げる
                        i2c.Out_Set(Out_Data,0,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;

                switch(kago) {
                    //腕を前に動かす
                    case 0:
                        //爪閉じる
                        i2c.Out_Set(Out_Data,1,0);
                        //腕上げる
                        i2c.Out_Set(Out_Data,0,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 {

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

                if(sbdbt.R1) {
                    //腕おろす
                    i2c.Out_Set(Out_Data,0,1);
                    //爪閉じる
                    i2c.Out_Set(Out_Data,1,0);
                }
                //腕あげる
                else i2c.Out_Set(Out_Data,0,0);

                if(disturb_f) {
                    //真ん中で止まる用
                    short_f=1;
                    switch(disturb) {
                        //腕を前に動かす
                        case 0:
                            //爪開く
                            i2c.Out_Set(Out_Data,1,1);
                            //腕上げる
                            i2c.Out_Set(Out_Data,0,0);

                            MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
                            MD_SET_PWM  (MD_Data,4,60);

                            if(max_lim==1) {
                                disturb++;
                                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) {
                                disturb++;
                                tim.reset();
                            }
                            break;

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

                        case 3:
                            //腕上げる
                            i2c.Out_Set(Out_Data,0,0);

                            //赤ゾーン
                            if( yaw<=90 || 270<yaw ) {
                                MD_SET_PWM(MD_Data,4,20);
                                if(RIGHTkey && max_lim==0) MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
                                else if(LEFTkey && start_lim==0) MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
                                else {
                                    MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
                                    MD_SET_PWM  (MD_Data,4,0);
                                }
                            }
                            //青ゾーン
                            else if( 90<yaw && yaw<=270 ) {
                                MD_SET_PWM(MD_Data,4,20);
                                if(LEFTkey && max_lim==0) MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
                                else if(RIGHTkey && start_lim==0) MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
                                else {
                                    MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
                                    MD_SET_PWM  (MD_Data,4,0);
                                }
                            }
                    }
                } //if(disturb_f)

                //腕真ん中で停止
                else if(short_f) {
                    //爪開く
                    i2c.Out_Set(Out_Data,1,1);

                    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 {
                    //爪開く
                    i2c.Out_Set(Out_Data,1,1);

                    //赤ゾーン
                    if( yaw<=90 || 270<yaw ) {
                        MD_SET_PWM(MD_Data,4,20);
                        if(RIGHTkey && max_lim==0) MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
                        else if(LEFTkey && start_lim==0) MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
                        else {
                            MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
                            MD_SET_PWM  (MD_Data,4,0);
                        }
                    }
                    //青ゾーン
                    else if( 90<yaw && yaw<=270 ) {
                        MD_SET_PWM(MD_Data,4,20);
                        if(LEFTkey && max_lim==0) MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
                        else if(RIGHTkey && start_lim==0) MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
                        else {
                            MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
                            MD_SET_PWM  (MD_Data,4,0);
                        }
                    }

                    //止まる
                    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);
                }
            }
        }
        //非常停止
        else {
            for(int i=0; i<8; i++) {
                MD_SET_DRIVE(MD_Data,i,MD_BRAKE);
                MD_SET_PWM  (MD_Data,i,0);
            }
        }

        /* --------------電磁弁系-------------------- */

        /*タオルを引っ張るやつ*/
        if(sbdbt.R2)i2c.Out_Set(Out_Data,3,1);
        else i2c.Out_Set(Out_Data,3,0);

        //タオル展開
        if(unfold_f) i2c.Out_Set(Out_Data,5,1);
        else i2c.Out_Set(Out_Data,5,0);

        //エアー解放
        if(release_f) {
            PALETTE(YELLOW);
            i2c.Out_Set(Out_Data,4,1);
        } else i2c.Out_Set(Out_Data,4,0);

        if(!rod_hit) PALETTE(CYAN);

        /*タオル掛け*/
        if(sbdbt.L2) i2c.Out_Set(Out_Data,2,1);
        else i2c.Out_Set(Out_Data,2,0);

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

        //モータ出力
        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;
        rod_hit = (In_Data+8)->in_data;


        /* ----------------ボタン系--------------- */
        //角度リセット
        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 if(sbdbt.SELECT) {
            if(once==0) {
                release_f++;
                release_f%=2;
                once++;
            }
        } else if(sbdbt.PS) {
            if(once==0) {
                disturb=0;
                disturb_f++;
                disturb_f%=2;
                once++;
            }
        } else once=0;

        //半自動のデバック
//            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 %.1f",yaw, bno.euler.yaw);
//            pc.printf("%d %d",keep_tim[0].read_ms(),keep_f[0]);
//        pc.printf("LX:%d, LY:%d, RX:%d, RY:%d",sbdbt.LX, sbdbt.LY, sbdbt.RX, sbdbt.RY);
//        for(int i=0; i<9; i++)
//            pc.printf("%d ",(In_Data+i)->in_data);
//        pc.printf("\n\r");
    }     // 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      ==  ハンガー展開
*/