File content as of revision 8:b79d21c8178b:

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

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

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

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

//y_deg: ジャイロの生の値
//yaw: 実際に使う角度
double y_deg[2], yaw=def_val;

//ジャイロさんぷりんぐ用
double sum=0, ave=0;
int offset_cnt=0;

//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()
{
    //flipper1.attach(&flip1, 0.001);    // 割り込み

    while(1) {
        //自動系のタイマースタート
        tim.start();
        drift_tim.start();
        mpu.loop();

        //ジャイロから値を取得
        y_deg[1] = y_deg[0];
        y_deg[0] = (double)mpu.ypr[0] * 180 / M_PI;

        //180～-180をまたいだ時
        if(y_deg[1]<-90 && y_deg[0]>90) y_deg[1]+=360;
        else if(y_deg[1]>90 && y_deg[0]<-90) y_deg[1]-=360;

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

            //誤差をさんぷりんぐ
            if(offset_cnt<1000) {
                PALETTE(YELLOW);
                if(offset_cnt!=0) {
                    sum+=(y_deg[0]-y_deg[1]);
                    ave=sum/offset_cnt;
                }
                offset_cnt++;
            } else {
                //ゾーン対応
#ifdef  _RED_ZONE_
                PALETTE(RED);
#else
                PALETTE(BLUE);
#endif
                yaw -= (y_deg[0]-y_deg[1])-ave;
            }

            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 || spull_f) {
                    PALETTE(GREEN);
                    short_f=1;
                    switch(grab) {
                        //腕を前に動かす
                        case 0:
                            //爪開く
                            i2c.Out_Set(Out_Data,1,1);

                            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,40);

                            if(spull_f) {
                                if(short_lim==1) {
                                    grab++;
                                    back_f=0;
                                    tim.reset();
                                } else if(max_lim) back_f=1;
                            } else {
                                if(max_lim==1) {
                                    grab++;
                                    back_f=0;
                                    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,40);
                            if(start_lim==1) {
                                grab_f=0;
                                spull_f=0;
                                tim.reset();
                            }
                            break;
                    }
                } //if(grab_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,40);

                            if(max_lim==1) {
                                pull++;
                                back_f=0;
                                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,40);
                            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,40);
                            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) {
                                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,80);
                            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_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;
                        MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
                        MD_SET_PWM  (MD_Data,4,20);
                    } 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;

                    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("yaw:%.1f dif:%.1f",yaw,dif_val);
            pc.printf("0:%.1f 1:%.1f yaw:%.1f dif:%.1f",y_deg[0],y_deg[1],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      ==  ハンガー展開
*/
/*その他注意点*/
/*
動かすときはジャイロの値を安定させるためLEDが赤から緑になるまで待つ
メカナムは振動が大きいせいかナット等が外れやすいため、定期的に確認をする（特に足回り）
あまり急発進をさせない（たまに暴走）
暴走したときはコントローラーを動かしたら治るはず
コントローラー接続が切れたらSTMの黒い奴を押す（わからないときは近くの回路班へ）
*/