File content as of revision 1:5f5d47a593bc:

#include "mbed.h"
#include "Omuni.h"
#include "debug.h"
#include "User.h"
#include "DualShockMod.h"

//永井ここから
//#include "kyuuban.h"
//#include "QEI.h"

//Timer enc_tim;  //Timerクラスのインスタンス（実体）
//QEI qei(enc1,enc2,D10,800,&enc_tim);  //class宣言
// qei.getPulses(); //Pulsesを返す

//ここまで


//serial通信
Serial pc(SERIAL_TX, SERIAL_RX);
Serial tsuushin(PC_12,PD_2);  

//オドメトリの出力
int ofsetX=posX;
int ofsetY=posY;  

//フラグ変数
FLG control_start = NACT;
FLG dash_start_flg = NACT;

//エンコーダー値の変更
FLG POS_E_SA = NACT;
FLG POS_E_TDR = NACT;
FLG POS_E_TDR_B = NACT;
FLG POS_E_HTY = NACT;
FLG POS_E_PO1 = NACT;
FLG POS_E_PO2 = NACT;
FLG POS_E_PO3 = NACT;
FLG POS_E_PO4 = NACT;
FLG POS_E_PO5 = NACT;
FLG POS_E_PO6 = NACT;
FLG POS_E_PO7 = NACT;
FLG POS_E_PO8 = NACT;

//シーケンスポジション変数
typedef enum{
    POS_SA,
    POS_TDR,
    POS_TDR_B,
    POS_HTY,
    POS_PO1,
    POS_PO2,
    POS_PO3,
    POS_PO4,
    POS_PO5,
    POS_PO6,
    POS_PO7
}POS;

POS seq_pos=POS_SA;
    

//ポジション座標
#define SA_X 0.0
#define SA_Y 0.0

#define LA_TSUDURA_X -1400.0
#define LA_TSUDURA_Y 0.0

#define LA_BACK_X 1200.0
#define LA_BACK_Y 0.0

#define LA_1_X 0.0
#define LA_1_Y -3000.0

#define LA_2_X -800.0
#define LA_2_Y 0.0

#define LA_3_X 0.0
#define LA_3_Y -1100.0

#define LA_4_X -1700.0
#define LA_4_Y 0.0

#define LA_5_X  0.0
#define LA_5_Y 500.0

#define LA_6_X -1200.0
#define LA_6_Y 0.0

#define LA_7_X 0.0
#define LA_7_Y -300.0

#define LA_8_X 500.0
#define LA_8_Y 0.0

//メインシーケンス
typedef enum{
    SA_WAIT,
    POS_TSUDURA,
    POS_TSUDURA_B,
    POS_HITUYOU,
    POS_P1,
    POS_P2,
    POS_P3,
    POS_P4,
    POS_P5,
    POS_P6,
    POS_P7,
    POS_P8,
    POS_P9,
    POS_P10,
    POS_P11,
    POS_S
}MAIN_SEQUENCE;

MAIN_SEQUENCE main_sequence;

//サブシーケンス
typedef enum{
    KYUBAN_STOP
}SUB_SEQUENCE;

SUB_SEQUENCE sub_sequence;

//エンコーダー誤差
#define POSITION_RANGE 20



int main(void){    
    motor_init();           //cwなどの初期化
    while(1){
        //エンコーダーの値
        pc.printf("posX=%d,posY=%d\n\r",posX,posY);
        uint8_t InitDS(Serial* f_serial);
        void getPOSdata(void);
        tsuushin.baud(115200);    
        InitDS(&tsuushin);
        tsuushin.attach(&getPOSdata, Serial::RxIrq); //受信したら割り込み開始           
        //スタートボタンで制御開始（つづら）
        if(LIMIT2 == 0 && control_start==NACT){
            control_start = ACT;
            //エンコーダーの原点再設定
            
            //シーケンス設定
            main_sequence = SA_WAIT;
        }
        if(control_start == ACT){//（つづら）
            //座標情報取得
            //getMachinePosition();
            if(posX < (SA_X+POSITION_RANGE) &&  posX > (SA_X-POSITION_RANGE)){
                //現在地ＳＡ
                if(posY  < (SA_Y+POSITION_RANGE) && posY > (SA_Y-POSITION_RANGE) && POS_E_SA == NACT){
                    POS_E_SA = ACT;
                    POS_E_TDR = ACT;
                    seq_pos = POS_SA;
                }
                else if(posX < (LA_TSUDURA_X+POSITION_RANGE) && posX > (LA_TSUDURA_X-POSITION_RANGE) && POS_E_TDR == ACT){
                    POS_E_TDR = NACT;
                    POS_E_TDR_B = ACT;
                    seq_pos = POS_TDR;
                }
                else if(posX-ofsetX < (LA_BACK_X+POSITION_RANGE) && posX-ofsetX > (LA_BACK_X-POSITION_RANGE) && POS_E_TDR_B == ACT){
                    POS_E_TDR_B = NACT;
                    seq_pos = POS_TDR_B;
                }
            }
        }
            //プリセットポジション判定
        //スタートボタンで制御開始（必要ボックス）
        if(LIMIT1 ==1 && dash_start_flg==NACT){
            dash_start_flg = ACT;
            //エンコーダーの原点再設定
            //シーケンス設定
            main_sequence = SA_WAIT;
        }
        if(dash_start_flg == ACT){//（必要ボックス）
            //座標情報取得
            //getMachinePosition();    
            if(posX < (SA_X+POSITION_RANGE) &&  posX > (SA_X-POSITION_RANGE)){
                //現在地ＳＡ
                if(posY  < (SA_Y+POSITION_RANGE) && posY > (SA_Y-POSITION_RANGE) && POS_E_HTY == NACT){
                    POS_E_HTY = ACT;
                    POS_E_PO1 = ACT;
                    seq_pos = POS_SA;
                }
                if(posY < (LA_1_Y+POSITION_RANGE) && posY > (LA_1_Y-POSITION_RANGE) && POS_E_PO1 == ACT){
                    POS_E_PO1 = NACT;
                    POS_E_PO2 = ACT;
                    seq_pos = POS_HTY;
                }
                if(posX-ofsetX < (LA_2_X+POSITION_RANGE) && posX-ofsetX > (LA_2_X-POSITION_RANGE) && POS_E_PO2 == ACT){
                    POS_E_PO2 = NACT;
                    POS_E_PO3 = ACT;
                    seq_pos = POS_PO1;   
                }
                if(posY-ofsetY < (LA_3_Y+POSITION_RANGE) && posY-ofsetY > (LA_3_Y-POSITION_RANGE) && POS_E_PO3 == ACT){
                    POS_E_PO3 = NACT;
                    POS_E_PO4 = ACT;
                    seq_pos = POS_PO2;  
                }
                if(posX-ofsetX < (LA_4_X+POSITION_RANGE) && posX-ofsetX > (LA_4_X-POSITION_RANGE) && POS_E_PO4 == ACT){
                    POS_E_PO4 = NACT;
                    POS_E_PO5 = ACT;
                    seq_pos = POS_PO3;  
                }
                if(posY-ofsetY < (LA_5_Y+POSITION_RANGE) && posY-ofsetY > (LA_5_Y-POSITION_RANGE) && POS_E_PO5 == ACT){
                    POS_E_PO5 = NACT;
                    POS_E_PO6 = ACT;
                    seq_pos = POS_PO4;  
                }
                if(posX-ofsetX < (LA_6_X+POSITION_RANGE) && posX-ofsetX > (LA_6_X-POSITION_RANGE) && POS_E_PO6 == ACT){
                    POS_E_PO6 = NACT;
                    POS_E_PO7 = ACT;
                    seq_pos = POS_PO5;  
                }
                if(posY-ofsetY < (LA_7_Y+POSITION_RANGE) && posY-ofsetY > (LA_7_Y-POSITION_RANGE) && POS_E_PO7 == ACT){
                    POS_E_PO7 = NACT;
                    POS_E_PO8 = ACT;
                    seq_pos = POS_PO6;  
                }
                if(posX-ofsetX < (LA_8_X+POSITION_RANGE) && posX-ofsetX > (LA_8_X-POSITION_RANGE) && POS_E_PO8 == ACT){
                    POS_E_PO8 = NACT;
                    seq_pos = POS_PO7;  
                }
            }
        }
        switch(main_sequence){
        case SA_WAIT:
            if(seq_pos == POS_SA && control_start == ACT)main_sequence = POS_TSUDURA;
            if(seq_pos == POS_SA && dash_start_flg == ACT)main_sequence = POS_HITUYOU;
            break;
        //つづら
        case POS_TSUDURA:
            motor_lf();
            if(seq_pos == POS_TDR)main_sequence = POS_TSUDURA_B,ofsetX=posX;
            break;
        case POS_TSUDURA_B:
            motor_rf();
            if(seq_pos == POS_TDR_B)main_sequence=POS_S;
            break;
        //必要ボックス
        case POS_HITUYOU:
            motor_ff();
            if(seq_pos == POS_HTY)main_sequence=POS_P1;
            break;
        case POS_P1:
            motor_dr();
            if(LIMIT2==0 && LIMIT3==0)main_sequence=POS_P2,ofsetX=posX;
            break;
        case POS_P2:
            motor_lf();
            if(seq_pos == POS_PO1)main_sequence = POS_P3,ofsetY=posY;
            break;
        case POS_P3:
            motor_ff();
            if(seq_pos == POS_PO2)main_sequence = POS_P4;
            break;
        case POS_P4:
            motor_fs();
            if(LIMIT2 == 0)main_sequence = POS_P5,ofsetX=posX;//押し付けるには？
            break;
        case POS_P5:
            motor_lf();
            if(/*LIMIT2 == 1 &&*/ seq_pos == POS_PO3)main_sequence = POS_P6;
            //if(limit_switch1 == 0)main_sequence = POS_4;
            break;
        case POS_P6:
            motor_dl();
            if(LIMIT1 == 1 && LIMIT2 == 0)main_sequence = POS_P7,ofsetY=posY;
            break;
        case POS_P7:
            motor_bf();
            if(seq_pos == POS_PO4)main_sequence = POS_P8,ofsetX=posX;
            break;
        case POS_P8:
            motor_lf();
            if(seq_pos == POS_PO5)main_sequence = POS_P9,ofsetY=posY;
            break;
        case POS_P9:
            motor_fs();
            if(seq_pos == POS_PO6)main_sequence = POS_P10,ofsetX=posX;
            break;
        case POS_P10:
            motor_ls();
            if(LIMIT1 == 1)main_sequence = POS_P11,ofsetX=posX;
            break;
        case POS_P11:
            motor_rf();
            if(seq_pos == POS_PO7)main_sequence = POS_S;
        case POS_S:
            motor_s();
            break;
        
        }
    }
}