main.cpp

00001 #include "mbed.h"
00002 #include "Omuni.h"
00003 #include "debug.h"
00004 #include "User.h"
00005 #include "DualShockMod.h"
00006 
00007 //永井ここから
00008 //#include "kyuuban.h"
00009 //#include "QEI.h"
00010 
00011 //Timer enc_tim;  //Timerクラスのインスタンス（実体）
00012 //QEI qei(enc1,enc2,D10,800,&enc_tim);  //class宣言
00013 // qei.getPulses(); //Pulsesを返す
00014 
00015 //ここまで
00016 
00017 
00018 //serial通信
00019 Serial pc(SERIAL_TX, SERIAL_RX);
00020 Serial tsuushin(PC_12,PD_2);  
00021 
00022 //オドメトリの出力
00023 int ofsetX=posX;
00024 int ofsetY=posY;  
00025 
00026 //フラグ変数
00027 FLG control_start = NACT;
00028 FLG dash_start_flg = NACT;
00029 
00030 //エンコーダー値の変更
00031 FLG POS_E_SA = NACT;
00032 FLG POS_E_TDR = NACT;
00033 FLG POS_E_TDR_B = NACT;
00034 FLG POS_E_HTY = NACT;
00035 FLG POS_E_PO1 = NACT;
00036 FLG POS_E_PO2 = NACT;
00037 FLG POS_E_PO3 = NACT;
00038 FLG POS_E_PO4 = NACT;
00039 FLG POS_E_PO5 = NACT;
00040 FLG POS_E_PO6 = NACT;
00041 FLG POS_E_PO7 = NACT;
00042 FLG POS_E_PO8 = NACT;
00043 
00044 //シーケンスポジション変数
00045 typedef enum{
00046     POS_SA,
00047     POS_TDR,
00048     POS_TDR_B,
00049     POS_HTY,
00050     POS_PO1,
00051     POS_PO2,
00052     POS_PO3,
00053     POS_PO4,
00054     POS_PO5,
00055     POS_PO6,
00056     POS_PO7
00057 }POS;
00058 
00059 POS seq_pos=POS_SA;
00060     
00061 
00062 //ポジション座標
00063 #define SA_X 0.0
00064 #define SA_Y 0.0
00065 
00066 #define LA_TSUDURA_X -1400.0
00067 #define LA_TSUDURA_Y 0.0
00068 
00069 #define LA_BACK_X 1200.0
00070 #define LA_BACK_Y 0.0
00071 
00072 #define LA_1_X 0.0
00073 #define LA_1_Y -3000.0
00074 
00075 #define LA_2_X -800.0
00076 #define LA_2_Y 0.0
00077 
00078 #define LA_3_X 0.0
00079 #define LA_3_Y -1100.0
00080 
00081 #define LA_4_X -1700.0
00082 #define LA_4_Y 0.0
00083 
00084 #define LA_5_X  0.0
00085 #define LA_5_Y 500.0
00086 
00087 #define LA_6_X -1200.0
00088 #define LA_6_Y 0.0
00089 
00090 #define LA_7_X 0.0
00091 #define LA_7_Y -300.0
00092 
00093 #define LA_8_X 500.0
00094 #define LA_8_Y 0.0
00095 
00096 //メインシーケンス
00097 typedef enum{
00098     SA_WAIT,
00099     POS_TSUDURA,
00100     POS_TSUDURA_B,
00101     POS_HITUYOU,
00102     POS_P1,
00103     POS_P2,
00104     POS_P3,
00105     POS_P4,
00106     POS_P5,
00107     POS_P6,
00108     POS_P7,
00109     POS_P8,
00110     POS_P9,
00111     POS_P10,
00112     POS_P11,
00113     POS_S
00114 }MAIN_SEQUENCE;
00115 
00116 MAIN_SEQUENCE main_sequence;
00117 
00118 //サブシーケンス
00119 typedef enum{
00120     KYUBAN_STOP
00121 }SUB_SEQUENCE;
00122 
00123 SUB_SEQUENCE sub_sequence;
00124 
00125 //エンコーダー誤差
00126 #define POSITION_RANGE 20
00127 
00128 
00129 
00130 int main(void){    
00131     motor_init();           //cwなどの初期化
00132     while(1){
00133         //エンコーダーの値
00134         pc.printf("posX=%d,posY=%d\n\r",posX,posY);
00135         uint8_t InitDS(Serial* f_serial);
00136         void getPOSdata(void);
00137         tsuushin.baud(115200);    
00138         InitDS(&tsuushin);
00139         tsuushin.attach(&getPOSdata, Serial::RxIrq); //受信したら割り込み開始           
00140         //スタートボタンで制御開始（つづら）
00141         if(LIMIT2 == 0 && control_start==NACT){
00142             control_start = ACT;
00143             //エンコーダーの原点再設定
00144             
00145             //シーケンス設定
00146             main_sequence = SA_WAIT;
00147         }
00148         if(control_start == ACT){//（つづら）
00149             //座標情報取得
00150             //getMachinePosition();
00151             if(posX < (SA_X+POSITION_RANGE) &&  posX > (SA_X-POSITION_RANGE)){
00152                 //現在地ＳＡ
00153                 if(posY  < (SA_Y+POSITION_RANGE) && posY > (SA_Y-POSITION_RANGE) && POS_E_SA == NACT){
00154                     POS_E_SA = ACT;
00155                     POS_E_TDR = ACT;
00156                     seq_pos = POS_SA;
00157                 }
00158                 else if(posX < (LA_TSUDURA_X+POSITION_RANGE) && posX > (LA_TSUDURA_X-POSITION_RANGE) && POS_E_TDR == ACT){
00159                     POS_E_TDR = NACT;
00160                     POS_E_TDR_B = ACT;
00161                     seq_pos = POS_TDR;
00162                 }
00163                 else if(posX-ofsetX < (LA_BACK_X+POSITION_RANGE) && posX-ofsetX > (LA_BACK_X-POSITION_RANGE) && POS_E_TDR_B == ACT){
00164                     POS_E_TDR_B = NACT;
00165                     seq_pos = POS_TDR_B;
00166                 }
00167             }
00168         }
00169             //プリセットポジション判定
00170         //スタートボタンで制御開始（必要ボックス）
00171         if(LIMIT1 ==1 && dash_start_flg==NACT){
00172             dash_start_flg = ACT;
00173             //エンコーダーの原点再設定
00174             //シーケンス設定
00175             main_sequence = SA_WAIT;
00176         }
00177         if(dash_start_flg == ACT){//（必要ボックス）
00178             //座標情報取得
00179             //getMachinePosition();    
00180             if(posX < (SA_X+POSITION_RANGE) &&  posX > (SA_X-POSITION_RANGE)){
00181                 //現在地ＳＡ
00182                 if(posY  < (SA_Y+POSITION_RANGE) && posY > (SA_Y-POSITION_RANGE) && POS_E_HTY == NACT){
00183                     POS_E_HTY = ACT;
00184                     POS_E_PO1 = ACT;
00185                     seq_pos = POS_SA;
00186                 }
00187                 if(posY < (LA_1_Y+POSITION_RANGE) && posY > (LA_1_Y-POSITION_RANGE) && POS_E_PO1 == ACT){
00188                     POS_E_PO1 = NACT;
00189                     POS_E_PO2 = ACT;
00190                     seq_pos = POS_HTY;
00191                 }
00192                 if(posX-ofsetX < (LA_2_X+POSITION_RANGE) && posX-ofsetX > (LA_2_X-POSITION_RANGE) && POS_E_PO2 == ACT){
00193                     POS_E_PO2 = NACT;
00194                     POS_E_PO3 = ACT;
00195                     seq_pos = POS_PO1;   
00196                 }
00197                 if(posY-ofsetY < (LA_3_Y+POSITION_RANGE) && posY-ofsetY > (LA_3_Y-POSITION_RANGE) && POS_E_PO3 == ACT){
00198                     POS_E_PO3 = NACT;
00199                     POS_E_PO4 = ACT;
00200                     seq_pos = POS_PO2;  
00201                 }
00202                 if(posX-ofsetX < (LA_4_X+POSITION_RANGE) && posX-ofsetX > (LA_4_X-POSITION_RANGE) && POS_E_PO4 == ACT){
00203                     POS_E_PO4 = NACT;
00204                     POS_E_PO5 = ACT;
00205                     seq_pos = POS_PO3;  
00206                 }
00207                 if(posY-ofsetY < (LA_5_Y+POSITION_RANGE) && posY-ofsetY > (LA_5_Y-POSITION_RANGE) && POS_E_PO5 == ACT){
00208                     POS_E_PO5 = NACT;
00209                     POS_E_PO6 = ACT;
00210                     seq_pos = POS_PO4;  
00211                 }
00212                 if(posX-ofsetX < (LA_6_X+POSITION_RANGE) && posX-ofsetX > (LA_6_X-POSITION_RANGE) && POS_E_PO6 == ACT){
00213                     POS_E_PO6 = NACT;
00214                     POS_E_PO7 = ACT;
00215                     seq_pos = POS_PO5;  
00216                 }
00217                 if(posY-ofsetY < (LA_7_Y+POSITION_RANGE) && posY-ofsetY > (LA_7_Y-POSITION_RANGE) && POS_E_PO7 == ACT){
00218                     POS_E_PO7 = NACT;
00219                     POS_E_PO8 = ACT;
00220                     seq_pos = POS_PO6;  
00221                 }
00222                 if(posX-ofsetX < (LA_8_X+POSITION_RANGE) && posX-ofsetX > (LA_8_X-POSITION_RANGE) && POS_E_PO8 == ACT){
00223                     POS_E_PO8 = NACT;
00224                     seq_pos = POS_PO7;  
00225                 }
00226             }
00227         }
00228         switch(main_sequence){
00229         case SA_WAIT:
00230             if(seq_pos == POS_SA && control_start == ACT)main_sequence = POS_TSUDURA;
00231             if(seq_pos == POS_SA && dash_start_flg == ACT)main_sequence = POS_HITUYOU;
00232             break;
00233         //つづら
00234         case POS_TSUDURA:
00235             motor_lf();
00236             if(seq_pos == POS_TDR)main_sequence = POS_TSUDURA_B,ofsetX=posX;
00237             break;
00238         case POS_TSUDURA_B:
00239             motor_rf();
00240             if(seq_pos == POS_TDR_B)main_sequence=POS_S;
00241             break;
00242         //必要ボックス
00243         case POS_HITUYOU:
00244             motor_ff();
00245             if(seq_pos == POS_HTY)main_sequence=POS_P1;
00246             break;
00247         case POS_P1:
00248             motor_dr();
00249             if(LIMIT2==0 && LIMIT3==0)main_sequence=POS_P2,ofsetX=posX;
00250             break;
00251         case POS_P2:
00252             motor_lf();
00253             if(seq_pos == POS_PO1)main_sequence = POS_P3,ofsetY=posY;
00254             break;
00255         case POS_P3:
00256             motor_ff();
00257             if(seq_pos == POS_PO2)main_sequence = POS_P4;
00258             break;
00259         case POS_P4:
00260             motor_fs();
00261             if(LIMIT2 == 0)main_sequence = POS_P5,ofsetX=posX;//押し付けるには？
00262             break;
00263         case POS_P5:
00264             motor_lf();
00265             if(/*LIMIT2 == 1 &&*/ seq_pos == POS_PO3)main_sequence = POS_P6;
00266             //if(limit_switch1 == 0)main_sequence = POS_4;
00267             break;
00268         case POS_P6:
00269             motor_dl();
00270             if(LIMIT1 == 1 && LIMIT2 == 0)main_sequence = POS_P7,ofsetY=posY;
00271             break;
00272         case POS_P7:
00273             motor_bf();
00274             if(seq_pos == POS_PO4)main_sequence = POS_P8,ofsetX=posX;
00275             break;
00276         case POS_P8:
00277             motor_lf();
00278             if(seq_pos == POS_PO5)main_sequence = POS_P9,ofsetY=posY;
00279             break;
00280         case POS_P9:
00281             motor_fs();
00282             if(seq_pos == POS_PO6)main_sequence = POS_P10,ofsetX=posX;
00283             break;
00284         case POS_P10:
00285             motor_ls();
00286             if(LIMIT1 == 1)main_sequence = POS_P11,ofsetX=posX;
00287             break;
00288         case POS_P11:
00289             motor_rf();
00290             if(seq_pos == POS_PO7)main_sequence = POS_S;
00291         case POS_S:
00292             motor_s();
00293             break;
00294         
00295         }
00296     }
00297 }