yei

Dependencies:   interface mbed enc_1multi calPID motorout KondoServoLibrary

Fork of cat18_operate by Catch the GIANT Caplico!

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