謙一 永井 / Mbed 2 deprecated SRC2018Auto

0910

Dependencies:   mbed QEI2

main.cpp@1:5f5d47a593bc, 2019-09-10 (annotated)

Committer:
kenboh
Date:
Tue Sep 10 09:30:41 2019 +0000
Revision:
1:5f5d47a593bc
Parent:
0:2a0c62e53e9c 
SRC2018Auto

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Hase_jun 0:2a0c62e53e9c 1 #include "mbed.h"
Hase_jun 0:2a0c62e53e9c 2 #include "Omuni.h"
Hase_jun 0:2a0c62e53e9c 3 #include "debug.h"
Hase_jun 0:2a0c62e53e9c 4 #include "User.h"
Hase_jun 0:2a0c62e53e9c 5 #include "DualShockMod.h"
Hase_jun 0:2a0c62e53e9c 6
kenboh 1:5f5d47a593bc 7 //永井ここから
kenboh 1:5f5d47a593bc 8 //#include "kyuuban.h"
kenboh 1:5f5d47a593bc 9 //#include "QEI.h"
kenboh 1:5f5d47a593bc 10
kenboh 1:5f5d47a593bc 11 //Timer enc_tim; //Timerクラスのインスタンス（実体）
kenboh 1:5f5d47a593bc 12 //QEI qei(enc1,enc2,D10,800,&enc_tim); //class宣言
kenboh 1:5f5d47a593bc 13 // qei.getPulses(); //Pulsesを返す
kenboh 1:5f5d47a593bc 14
kenboh 1:5f5d47a593bc 15 //ここまで
kenboh 1:5f5d47a593bc 16
kenboh 1:5f5d47a593bc 17
Hase_jun 0:2a0c62e53e9c 18 //serial通信
Hase_jun 0:2a0c62e53e9c 19 Serial pc(SERIAL_TX, SERIAL_RX);
Hase_jun 0:2a0c62e53e9c 20 Serial tsuushin(PC_12,PD_2);
Hase_jun 0:2a0c62e53e9c 21
Hase_jun 0:2a0c62e53e9c 22 //オドメトリの出力
Hase_jun 0:2a0c62e53e9c 23 int ofsetX=posX;
Hase_jun 0:2a0c62e53e9c 24 int ofsetY=posY;
Hase_jun 0:2a0c62e53e9c 25
Hase_jun 0:2a0c62e53e9c 26 //フラグ変数
Hase_jun 0:2a0c62e53e9c 27 FLG control_start = NACT;
Hase_jun 0:2a0c62e53e9c 28 FLG dash_start_flg = NACT;
Hase_jun 0:2a0c62e53e9c 29
Hase_jun 0:2a0c62e53e9c 30 //エンコーダー値の変更
Hase_jun 0:2a0c62e53e9c 31 FLG POS_E_SA = NACT;
Hase_jun 0:2a0c62e53e9c 32 FLG POS_E_TDR = NACT;
Hase_jun 0:2a0c62e53e9c 33 FLG POS_E_TDR_B = NACT;
Hase_jun 0:2a0c62e53e9c 34 FLG POS_E_HTY = NACT;
Hase_jun 0:2a0c62e53e9c 35 FLG POS_E_PO1 = NACT;
Hase_jun 0:2a0c62e53e9c 36 FLG POS_E_PO2 = NACT;
Hase_jun 0:2a0c62e53e9c 37 FLG POS_E_PO3 = NACT;
Hase_jun 0:2a0c62e53e9c 38 FLG POS_E_PO4 = NACT;
Hase_jun 0:2a0c62e53e9c 39 FLG POS_E_PO5 = NACT;
Hase_jun 0:2a0c62e53e9c 40 FLG POS_E_PO6 = NACT;
Hase_jun 0:2a0c62e53e9c 41 FLG POS_E_PO7 = NACT;
Hase_jun 0:2a0c62e53e9c 42 FLG POS_E_PO8 = NACT;
Hase_jun 0:2a0c62e53e9c 43
Hase_jun 0:2a0c62e53e9c 44 //シーケンスポジション変数
Hase_jun 0:2a0c62e53e9c 45 typedef enum{
Hase_jun 0:2a0c62e53e9c 46 POS_SA,
Hase_jun 0:2a0c62e53e9c 47 POS_TDR,
Hase_jun 0:2a0c62e53e9c 48 POS_TDR_B,
Hase_jun 0:2a0c62e53e9c 49 POS_HTY,
Hase_jun 0:2a0c62e53e9c 50 POS_PO1,
Hase_jun 0:2a0c62e53e9c 51 POS_PO2,
Hase_jun 0:2a0c62e53e9c 52 POS_PO3,
Hase_jun 0:2a0c62e53e9c 53 POS_PO4,
Hase_jun 0:2a0c62e53e9c 54 POS_PO5,
Hase_jun 0:2a0c62e53e9c 55 POS_PO6,
Hase_jun 0:2a0c62e53e9c 56 POS_PO7
Hase_jun 0:2a0c62e53e9c 57 }POS;
Hase_jun 0:2a0c62e53e9c 58
Hase_jun 0:2a0c62e53e9c 59 POS seq_pos=POS_SA;
Hase_jun 0:2a0c62e53e9c 60
Hase_jun 0:2a0c62e53e9c 61
Hase_jun 0:2a0c62e53e9c 62 //ポジション座標
Hase_jun 0:2a0c62e53e9c 63 #define SA_X 0.0
Hase_jun 0:2a0c62e53e9c 64 #define SA_Y 0.0
Hase_jun 0:2a0c62e53e9c 65
Hase_jun 0:2a0c62e53e9c 66 #define LA_TSUDURA_X -1400.0
Hase_jun 0:2a0c62e53e9c 67 #define LA_TSUDURA_Y 0.0
Hase_jun 0:2a0c62e53e9c 68
Hase_jun 0:2a0c62e53e9c 69 #define LA_BACK_X 1200.0
Hase_jun 0:2a0c62e53e9c 70 #define LA_BACK_Y 0.0
Hase_jun 0:2a0c62e53e9c 71
Hase_jun 0:2a0c62e53e9c 72 #define LA_1_X 0.0
Hase_jun 0:2a0c62e53e9c 73 #define LA_1_Y -3000.0
Hase_jun 0:2a0c62e53e9c 74
Hase_jun 0:2a0c62e53e9c 75 #define LA_2_X -800.0
Hase_jun 0:2a0c62e53e9c 76 #define LA_2_Y 0.0
Hase_jun 0:2a0c62e53e9c 77
Hase_jun 0:2a0c62e53e9c 78 #define LA_3_X 0.0
Hase_jun 0:2a0c62e53e9c 79 #define LA_3_Y -1100.0
Hase_jun 0:2a0c62e53e9c 80
Hase_jun 0:2a0c62e53e9c 81 #define LA_4_X -1700.0
Hase_jun 0:2a0c62e53e9c 82 #define LA_4_Y 0.0
Hase_jun 0:2a0c62e53e9c 83
Hase_jun 0:2a0c62e53e9c 84 #define LA_5_X 0.0
Hase_jun 0:2a0c62e53e9c 85 #define LA_5_Y 500.0
Hase_jun 0:2a0c62e53e9c 86
Hase_jun 0:2a0c62e53e9c 87 #define LA_6_X -1200.0
Hase_jun 0:2a0c62e53e9c 88 #define LA_6_Y 0.0
Hase_jun 0:2a0c62e53e9c 89
Hase_jun 0:2a0c62e53e9c 90 #define LA_7_X 0.0
Hase_jun 0:2a0c62e53e9c 91 #define LA_7_Y -300.0
Hase_jun 0:2a0c62e53e9c 92
Hase_jun 0:2a0c62e53e9c 93 #define LA_8_X 500.0
Hase_jun 0:2a0c62e53e9c 94 #define LA_8_Y 0.0
Hase_jun 0:2a0c62e53e9c 95
Hase_jun 0:2a0c62e53e9c 96 //メインシーケンス
Hase_jun 0:2a0c62e53e9c 97 typedef enum{
Hase_jun 0:2a0c62e53e9c 98 SA_WAIT,
Hase_jun 0:2a0c62e53e9c 99 POS_TSUDURA,
Hase_jun 0:2a0c62e53e9c 100 POS_TSUDURA_B,
Hase_jun 0:2a0c62e53e9c 101 POS_HITUYOU,
Hase_jun 0:2a0c62e53e9c 102 POS_P1,
Hase_jun 0:2a0c62e53e9c 103 POS_P2,
Hase_jun 0:2a0c62e53e9c 104 POS_P3,
Hase_jun 0:2a0c62e53e9c 105 POS_P4,
Hase_jun 0:2a0c62e53e9c 106 POS_P5,
Hase_jun 0:2a0c62e53e9c 107 POS_P6,
Hase_jun 0:2a0c62e53e9c 108 POS_P7,
Hase_jun 0:2a0c62e53e9c 109 POS_P8,
Hase_jun 0:2a0c62e53e9c 110 POS_P9,
Hase_jun 0:2a0c62e53e9c 111 POS_P10,
Hase_jun 0:2a0c62e53e9c 112 POS_P11,
Hase_jun 0:2a0c62e53e9c 113 POS_S
Hase_jun 0:2a0c62e53e9c 114 }MAIN_SEQUENCE;
Hase_jun 0:2a0c62e53e9c 115
Hase_jun 0:2a0c62e53e9c 116 MAIN_SEQUENCE main_sequence;
Hase_jun 0:2a0c62e53e9c 117
Hase_jun 0:2a0c62e53e9c 118 //サブシーケンス
Hase_jun 0:2a0c62e53e9c 119 typedef enum{
Hase_jun 0:2a0c62e53e9c 120 KYUBAN_STOP
Hase_jun 0:2a0c62e53e9c 121 }SUB_SEQUENCE;
Hase_jun 0:2a0c62e53e9c 122
Hase_jun 0:2a0c62e53e9c 123 SUB_SEQUENCE sub_sequence;
Hase_jun 0:2a0c62e53e9c 124
Hase_jun 0:2a0c62e53e9c 125 //エンコーダー誤差
Hase_jun 0:2a0c62e53e9c 126 #define POSITION_RANGE 20
Hase_jun 0:2a0c62e53e9c 127
Hase_jun 0:2a0c62e53e9c 128
Hase_jun 0:2a0c62e53e9c 129
kenboh 1:5f5d47a593bc 130 int main(void){
Hase_jun 0:2a0c62e53e9c 131 motor_init(); //cwなどの初期化
Hase_jun 0:2a0c62e53e9c 132 while(1){
Hase_jun 0:2a0c62e53e9c 133 //エンコーダーの値
Hase_jun 0:2a0c62e53e9c 134 pc.printf("posX=%d,posY=%d\n\r",posX,posY);
Hase_jun 0:2a0c62e53e9c 135 uint8_t InitDS(Serial* f_serial);
Hase_jun 0:2a0c62e53e9c 136 void getPOSdata(void);
Hase_jun 0:2a0c62e53e9c 137 tsuushin.baud(115200);
Hase_jun 0:2a0c62e53e9c 138 InitDS(&tsuushin);
Hase_jun 0:2a0c62e53e9c 139 tsuushin.attach(&getPOSdata, Serial::RxIrq); //受信したら割り込み開始
Hase_jun 0:2a0c62e53e9c 140 //スタートボタンで制御開始（つづら）
Hase_jun 0:2a0c62e53e9c 141 if(LIMIT2 == 0 && control_start==NACT){
Hase_jun 0:2a0c62e53e9c 142 control_start = ACT;
Hase_jun 0:2a0c62e53e9c 143 //エンコーダーの原点再設定
Hase_jun 0:2a0c62e53e9c 144
Hase_jun 0:2a0c62e53e9c 145 //シーケンス設定
Hase_jun 0:2a0c62e53e9c 146 main_sequence = SA_WAIT;
Hase_jun 0:2a0c62e53e9c 147 }
Hase_jun 0:2a0c62e53e9c 148 if(control_start == ACT){//（つづら）
Hase_jun 0:2a0c62e53e9c 149 //座標情報取得
Hase_jun 0:2a0c62e53e9c 150 //getMachinePosition();
Hase_jun 0:2a0c62e53e9c 151 if(posX < (SA_X+POSITION_RANGE) && posX > (SA_X-POSITION_RANGE)){
Hase_jun 0:2a0c62e53e9c 152 //現在地ＳＡ
Hase_jun 0:2a0c62e53e9c 153 if(posY < (SA_Y+POSITION_RANGE) && posY > (SA_Y-POSITION_RANGE) && POS_E_SA == NACT){
Hase_jun 0:2a0c62e53e9c 154 POS_E_SA = ACT;
Hase_jun 0:2a0c62e53e9c 155 POS_E_TDR = ACT;
Hase_jun 0:2a0c62e53e9c 156 seq_pos = POS_SA;
Hase_jun 0:2a0c62e53e9c 157 }
Hase_jun 0:2a0c62e53e9c 158 else if(posX < (LA_TSUDURA_X+POSITION_RANGE) && posX > (LA_TSUDURA_X-POSITION_RANGE) && POS_E_TDR == ACT){
Hase_jun 0:2a0c62e53e9c 159 POS_E_TDR = NACT;
Hase_jun 0:2a0c62e53e9c 160 POS_E_TDR_B = ACT;
Hase_jun 0:2a0c62e53e9c 161 seq_pos = POS_TDR;
Hase_jun 0:2a0c62e53e9c 162 }
Hase_jun 0:2a0c62e53e9c 163 else if(posX-ofsetX < (LA_BACK_X+POSITION_RANGE) && posX-ofsetX > (LA_BACK_X-POSITION_RANGE) && POS_E_TDR_B == ACT){
Hase_jun 0:2a0c62e53e9c 164 POS_E_TDR_B = NACT;
Hase_jun 0:2a0c62e53e9c 165 seq_pos = POS_TDR_B;
Hase_jun 0:2a0c62e53e9c 166 }
Hase_jun 0:2a0c62e53e9c 167 }
Hase_jun 0:2a0c62e53e9c 168 }
Hase_jun 0:2a0c62e53e9c 169 //プリセットポジション判定
Hase_jun 0:2a0c62e53e9c 170 //スタートボタンで制御開始（必要ボックス）
Hase_jun 0:2a0c62e53e9c 171 if(LIMIT1 ==1 && dash_start_flg==NACT){
Hase_jun 0:2a0c62e53e9c 172 dash_start_flg = ACT;
Hase_jun 0:2a0c62e53e9c 173 //エンコーダーの原点再設定
Hase_jun 0:2a0c62e53e9c 174 //シーケンス設定
Hase_jun 0:2a0c62e53e9c 175 main_sequence = SA_WAIT;
Hase_jun 0:2a0c62e53e9c 176 }
Hase_jun 0:2a0c62e53e9c 177 if(dash_start_flg == ACT){//（必要ボックス）
Hase_jun 0:2a0c62e53e9c 178 //座標情報取得
Hase_jun 0:2a0c62e53e9c 179 //getMachinePosition();
Hase_jun 0:2a0c62e53e9c 180 if(posX < (SA_X+POSITION_RANGE) && posX > (SA_X-POSITION_RANGE)){
Hase_jun 0:2a0c62e53e9c 181 //現在地ＳＡ
Hase_jun 0:2a0c62e53e9c 182 if(posY < (SA_Y+POSITION_RANGE) && posY > (SA_Y-POSITION_RANGE) && POS_E_HTY == NACT){
Hase_jun 0:2a0c62e53e9c 183 POS_E_HTY = ACT;
Hase_jun 0:2a0c62e53e9c 184 POS_E_PO1 = ACT;
Hase_jun 0:2a0c62e53e9c 185 seq_pos = POS_SA;
Hase_jun 0:2a0c62e53e9c 186 }
Hase_jun 0:2a0c62e53e9c 187 if(posY < (LA_1_Y+POSITION_RANGE) && posY > (LA_1_Y-POSITION_RANGE) && POS_E_PO1 == ACT){
Hase_jun 0:2a0c62e53e9c 188 POS_E_PO1 = NACT;
Hase_jun 0:2a0c62e53e9c 189 POS_E_PO2 = ACT;
Hase_jun 0:2a0c62e53e9c 190 seq_pos = POS_HTY;
Hase_jun 0:2a0c62e53e9c 191 }
Hase_jun 0:2a0c62e53e9c 192 if(posX-ofsetX < (LA_2_X+POSITION_RANGE) && posX-ofsetX > (LA_2_X-POSITION_RANGE) && POS_E_PO2 == ACT){
Hase_jun 0:2a0c62e53e9c 193 POS_E_PO2 = NACT;
Hase_jun 0:2a0c62e53e9c 194 POS_E_PO3 = ACT;
Hase_jun 0:2a0c62e53e9c 195 seq_pos = POS_PO1;
Hase_jun 0:2a0c62e53e9c 196 }
Hase_jun 0:2a0c62e53e9c 197 if(posY-ofsetY < (LA_3_Y+POSITION_RANGE) && posY-ofsetY > (LA_3_Y-POSITION_RANGE) && POS_E_PO3 == ACT){
Hase_jun 0:2a0c62e53e9c 198 POS_E_PO3 = NACT;
Hase_jun 0:2a0c62e53e9c 199 POS_E_PO4 = ACT;
Hase_jun 0:2a0c62e53e9c 200 seq_pos = POS_PO2;
Hase_jun 0:2a0c62e53e9c 201 }
Hase_jun 0:2a0c62e53e9c 202 if(posX-ofsetX < (LA_4_X+POSITION_RANGE) && posX-ofsetX > (LA_4_X-POSITION_RANGE) && POS_E_PO4 == ACT){
Hase_jun 0:2a0c62e53e9c 203 POS_E_PO4 = NACT;
Hase_jun 0:2a0c62e53e9c 204 POS_E_PO5 = ACT;
Hase_jun 0:2a0c62e53e9c 205 seq_pos = POS_PO3;
Hase_jun 0:2a0c62e53e9c 206 }
Hase_jun 0:2a0c62e53e9c 207 if(posY-ofsetY < (LA_5_Y+POSITION_RANGE) && posY-ofsetY > (LA_5_Y-POSITION_RANGE) && POS_E_PO5 == ACT){
Hase_jun 0:2a0c62e53e9c 208 POS_E_PO5 = NACT;
Hase_jun 0:2a0c62e53e9c 209 POS_E_PO6 = ACT;
Hase_jun 0:2a0c62e53e9c 210 seq_pos = POS_PO4;
Hase_jun 0:2a0c62e53e9c 211 }
Hase_jun 0:2a0c62e53e9c 212 if(posX-ofsetX < (LA_6_X+POSITION_RANGE) && posX-ofsetX > (LA_6_X-POSITION_RANGE) && POS_E_PO6 == ACT){
Hase_jun 0:2a0c62e53e9c 213 POS_E_PO6 = NACT;
Hase_jun 0:2a0c62e53e9c 214 POS_E_PO7 = ACT;
Hase_jun 0:2a0c62e53e9c 215 seq_pos = POS_PO5;
Hase_jun 0:2a0c62e53e9c 216 }
Hase_jun 0:2a0c62e53e9c 217 if(posY-ofsetY < (LA_7_Y+POSITION_RANGE) && posY-ofsetY > (LA_7_Y-POSITION_RANGE) && POS_E_PO7 == ACT){
Hase_jun 0:2a0c62e53e9c 218 POS_E_PO7 = NACT;
Hase_jun 0:2a0c62e53e9c 219 POS_E_PO8 = ACT;
Hase_jun 0:2a0c62e53e9c 220 seq_pos = POS_PO6;
Hase_jun 0:2a0c62e53e9c 221 }
Hase_jun 0:2a0c62e53e9c 222 if(posX-ofsetX < (LA_8_X+POSITION_RANGE) && posX-ofsetX > (LA_8_X-POSITION_RANGE) && POS_E_PO8 == ACT){
Hase_jun 0:2a0c62e53e9c 223 POS_E_PO8 = NACT;
Hase_jun 0:2a0c62e53e9c 224 seq_pos = POS_PO7;
Hase_jun 0:2a0c62e53e9c 225 }
Hase_jun 0:2a0c62e53e9c 226 }
Hase_jun 0:2a0c62e53e9c 227 }
Hase_jun 0:2a0c62e53e9c 228 switch(main_sequence){
Hase_jun 0:2a0c62e53e9c 229 case SA_WAIT:
Hase_jun 0:2a0c62e53e9c 230 if(seq_pos == POS_SA && control_start == ACT)main_sequence = POS_TSUDURA;
Hase_jun 0:2a0c62e53e9c 231 if(seq_pos == POS_SA && dash_start_flg == ACT)main_sequence = POS_HITUYOU;
Hase_jun 0:2a0c62e53e9c 232 break;
Hase_jun 0:2a0c62e53e9c 233 //つづら
Hase_jun 0:2a0c62e53e9c 234 case POS_TSUDURA:
Hase_jun 0:2a0c62e53e9c 235 motor_lf();
Hase_jun 0:2a0c62e53e9c 236 if(seq_pos == POS_TDR)main_sequence = POS_TSUDURA_B,ofsetX=posX;
Hase_jun 0:2a0c62e53e9c 237 break;
Hase_jun 0:2a0c62e53e9c 238 case POS_TSUDURA_B:
Hase_jun 0:2a0c62e53e9c 239 motor_rf();
Hase_jun 0:2a0c62e53e9c 240 if(seq_pos == POS_TDR_B)main_sequence=POS_S;
Hase_jun 0:2a0c62e53e9c 241 break;
Hase_jun 0:2a0c62e53e9c 242 //必要ボックス
Hase_jun 0:2a0c62e53e9c 243 case POS_HITUYOU:
Hase_jun 0:2a0c62e53e9c 244 motor_ff();
Hase_jun 0:2a0c62e53e9c 245 if(seq_pos == POS_HTY)main_sequence=POS_P1;
Hase_jun 0:2a0c62e53e9c 246 break;
Hase_jun 0:2a0c62e53e9c 247 case POS_P1:
Hase_jun 0:2a0c62e53e9c 248 motor_dr();
Hase_jun 0:2a0c62e53e9c 249 if(LIMIT2==0 && LIMIT3==0)main_sequence=POS_P2,ofsetX=posX;
Hase_jun 0:2a0c62e53e9c 250 break;
Hase_jun 0:2a0c62e53e9c 251 case POS_P2:
Hase_jun 0:2a0c62e53e9c 252 motor_lf();
Hase_jun 0:2a0c62e53e9c 253 if(seq_pos == POS_PO1)main_sequence = POS_P3,ofsetY=posY;
Hase_jun 0:2a0c62e53e9c 254 break;
Hase_jun 0:2a0c62e53e9c 255 case POS_P3:
Hase_jun 0:2a0c62e53e9c 256 motor_ff();
Hase_jun 0:2a0c62e53e9c 257 if(seq_pos == POS_PO2)main_sequence = POS_P4;
Hase_jun 0:2a0c62e53e9c 258 break;
Hase_jun 0:2a0c62e53e9c 259 case POS_P4:
Hase_jun 0:2a0c62e53e9c 260 motor_fs();
Hase_jun 0:2a0c62e53e9c 261 if(LIMIT2 == 0)main_sequence = POS_P5,ofsetX=posX;//押し付けるには？
Hase_jun 0:2a0c62e53e9c 262 break;
Hase_jun 0:2a0c62e53e9c 263 case POS_P5:
Hase_jun 0:2a0c62e53e9c 264 motor_lf();
Hase_jun 0:2a0c62e53e9c 265 if(/*LIMIT2 == 1 &&*/ seq_pos == POS_PO3)main_sequence = POS_P6;
Hase_jun 0:2a0c62e53e9c 266 //if(limit_switch1 == 0)main_sequence = POS_4;
Hase_jun 0:2a0c62e53e9c 267 break;
Hase_jun 0:2a0c62e53e9c 268 case POS_P6:
Hase_jun 0:2a0c62e53e9c 269 motor_dl();
Hase_jun 0:2a0c62e53e9c 270 if(LIMIT1 == 1 && LIMIT2 == 0)main_sequence = POS_P7,ofsetY=posY;
Hase_jun 0:2a0c62e53e9c 271 break;
Hase_jun 0:2a0c62e53e9c 272 case POS_P7:
Hase_jun 0:2a0c62e53e9c 273 motor_bf();
Hase_jun 0:2a0c62e53e9c 274 if(seq_pos == POS_PO4)main_sequence = POS_P8,ofsetX=posX;
Hase_jun 0:2a0c62e53e9c 275 break;
Hase_jun 0:2a0c62e53e9c 276 case POS_P8:
Hase_jun 0:2a0c62e53e9c 277 motor_lf();
Hase_jun 0:2a0c62e53e9c 278 if(seq_pos == POS_PO5)main_sequence = POS_P9,ofsetY=posY;
Hase_jun 0:2a0c62e53e9c 279 break;
Hase_jun 0:2a0c62e53e9c 280 case POS_P9:
Hase_jun 0:2a0c62e53e9c 281 motor_fs();
Hase_jun 0:2a0c62e53e9c 282 if(seq_pos == POS_PO6)main_sequence = POS_P10,ofsetX=posX;
Hase_jun 0:2a0c62e53e9c 283 break;
Hase_jun 0:2a0c62e53e9c 284 case POS_P10:
Hase_jun 0:2a0c62e53e9c 285 motor_ls();
Hase_jun 0:2a0c62e53e9c 286 if(LIMIT1 == 1)main_sequence = POS_P11,ofsetX=posX;
Hase_jun 0:2a0c62e53e9c 287 break;
Hase_jun 0:2a0c62e53e9c 288 case POS_P11:
Hase_jun 0:2a0c62e53e9c 289 motor_rf();
Hase_jun 0:2a0c62e53e9c 290 if(seq_pos == POS_PO7)main_sequence = POS_S;
Hase_jun 0:2a0c62e53e9c 291 case POS_S:
Hase_jun 0:2a0c62e53e9c 292 motor_s();
Hase_jun 0:2a0c62e53e9c 293 break;
Hase_jun 0:2a0c62e53e9c 294
Hase_jun 0:2a0c62e53e9c 295 }
Hase_jun 0:2a0c62e53e9c 296 }
kenboh 1:5f5d47a593bc 297 }