File content as of revision 16:829b953d1ac1:

#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;

/*----------------------------------- 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;

        //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);

        if(release_f) PALETTE(YELLOW);

        //リミット
        for(int i=0; i<7; 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;

        //メカナム基本動作
        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) {
            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 {
            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);

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

        /* --------------電磁弁系-------------------- */
        i2c.Out_Set(Out_Data, 2, sbdbt.L2); //タオル掛け
        i2c.Out_Set(Out_Data, 4, sbdbt.R2); //タオルを引っ張るやつ
        i2c.Out_Set(Out_Data, 3, unfold_f); //タオル展開
        i2c.Out_Set(Out_Data, 5, release_f);//エアー解放

        //-------------- 出力 --------------/
        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;

        /* ----------------ボタン系--------------- */
        //角度リセット
        if(sbdbt.L1) {
            yaw=def_val;
            Turn_val=def_val;
        }
        if(CROSS) Toggle(&kago_f, CROSS); //かごを倒す
        else if(SQUARE) Toggle(&grab_f, SQUARE);  //タオルを掴んで動かす
        else if(TRIANGLE) Toggle(&spull_f, TRIANGLE);
        else if(CIRCLE) Toggle(&pull_f, CIRCLE);
        else if(sbdbt.START) Toggle(&unfold_f, sbdbt.START);
        else if(sbdbt.SELECT) Toggle(&release_f, sbdbt.SELECT);
        else if(sbdbt.PS) Toggle(&disturb_f, sbdbt.PS);
        else once=0;

        //半自動のデバック
        pc.printf("%d %d ",kago_f, grab_f);
        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]));
    }
}

void Toggle(int *variable, int val)
{
    static int data[2]= {0};

    if(once==0) {
        data[0]=0;
        once=1;
    }

    data[1]=data[0];
    data[0]=val;

    if(data[1]==0 && data[0]==1) {
        (*variable)++;
        (*variable)%=2;
    }
}