yei
Dependencies: interface mbed enc_1multi calPID motorout KondoServoLibrary
Fork of cat18_operate by
Diff: main.cpp
- Revision:
- 2:4515e8cc6b60
- Parent:
- 1:cc056c41ae30
- Child:
- 3:05b1dcb3634e
diff -r cc056c41ae30 -r 4515e8cc6b60 main.cpp --- a/main.cpp Fri Jul 06 02:53:59 2018 +0000 +++ b/main.cpp Sun Aug 05 03:32:55 2018 +0000 @@ -4,11 +4,11 @@ #include "go.h" #include "hand.h" #include "calplace.h" +#include "interrupt.h" +#include "position.h" ///setup関連を集めた void Setups(); void Tests(); -WorkPosi work[23];//ワークエリア6箇所+共通エリア17 -BoxPosi box[12]; //const int kWorkNum = 12;//ワークの数 int main() @@ -18,17 +18,25 @@ DEBUG("main start"); while(1) { int worknum = CalPickPlace();//目標ワーク計算 - if(Go(work[worknum]) != 0) continue;//ワークのところまで行く - Grab(work[worknum]); - int boxspace = CalThrowPlace(work[worknum].color);//シュート位置計算 - if(Go(box[boxspace]) != 0) continue;//ボックスに戻る(失敗(-1)したらcontinue) - Throw(); - box[boxspace].is_placed = 1;//置いたことを保存 + //ワークのところまで行く。腕も準備 + if(Go(work[worknum]) != 0) continue; + Close(); + Above(); + work[worknum].is_exist = 0; + int boxspace = CalPutPlace(work[worknum].color);//シュート位置計算 + //ボックスに行く(失敗(-1)したらcontinue) + if(Go(shootingbox[boxspace]) != 0) continue; + Open(); + Above(); + shootingbox[boxspace].is_exist = 1;//置いたことを保存 + shootingbox[boxspace].color = work[worknum].color; } } void Setups() { GoSetup(); + SetupPosition(); + InterruptSetup(); } void Tests() { @@ -36,7 +44,6 @@ count ++; if (count > 100000) { count =0; - } }