Diff: main.cpp

Revision:

16:b2358fd35999

Parent:

13:126c3f7f9b89

Child:

18:05f5a3323bda

--- a/main.cpp	Tue Aug 14 01:20:40 2018 +0000
+++ b/main.cpp	Tue Aug 14 09:16:55 2018 +0000
@@ -4,7 +4,7 @@
 #include "go.h"
 #include "servo.h"
 #include "interrupt.h"
-#include "position.h"
+#include "workposition.h"
 #include "controller.h"
 //debug用に追加
 #include "interface.h"
@@ -17,17 +17,7 @@
 {
     DEBUG("setup start\r\n");
     Setups();
-        GoToZeroSlowly();
-    
 
-    double posi[3] = {200,200,50};
-    SetNextRadRelative(0, -90*kDegreeToRad);
-    SetNextRadRelative(1, 150*kDegreeToRad);
-    SetNextRadRelative(3, 0*kDegreeToRad);
-    SetServoYawRad(0);
-    ServoMoveOnArm();
-    wait(1);
-    ServoMoveOnArm();
     //スタート指示受付
     while(CanStart() == 0) {};
     DEBUG("main start");
@@ -35,28 +25,33 @@
     int worknum = 0;
     while( (worknum = CalPickPlace()) != 23 ) {
         //ワークに向かう
+        DEBUG("worknum %d, color %d, area %d, position %f, %f, %f\r\n", worknum, work[worknum].color, work[worknum].areaname, work[worknum].position[0],work[worknum].position[1],work[worknum].position[2]);
+        DEBUG("go to work\r\n");
         if(Go(work[worknum]) != 0) continue;
-        DEBUG("finish 1\r\n");
+        DEBUG("close\r\n");
         //掴む
         Close();
-        DEBUG("finish 2\r\n");
+        DEBUG("above\r\n");
+        wait(1);
         //上に持ち上げる
         Above();
-        DEBUG("finish 3\r\n");
         //取り上げたことを保存
         work[worknum].is_exist = 0;
         //シュート位置計算
+        DEBUG("cal put place\r\n");
         int boxspace = CalPutPlace(work[worknum].color);
-        DEBUG("finish 4\r\n");
         //ボックスに行く(失敗(-1)したらcontinue)
+        DEBUG("go to box %d\r\n",boxspace);
         if(Go(shootingbox[boxspace]) != 0) continue;
-        DEBUG("finish 5\r\n");
+        DEBUG("open\r\n");
         //放す
         Open();
-        DEBUG("finish 6\r\n");
+        wait(0.5);
+        DEBUG("above\r\n");
         //上に持ち上げる
         Above();
-        DEBUG("finish 7\r\n");
+        wait(0.1);
+        DEBUG("finish\r\n");
         //置いたことを保存
         shootingbox[boxspace].is_exist = 1;
         shootingbox[boxspace].color = work[worknum].color;
@@ -66,88 +61,14 @@
 {
     GoSetup();
     SetupPosition();
-
+   StartCheck();
 }
 void Tests()
 {
     static int count = 0;
     count ++;
     if (count > 100000) {
+        
         count =0;
     }
-}
-
-#if 0
-
-#include <mbed.h>
-#include "debug.h"//DEBUG("",変数);でデバッグ。
-#include "state.h"
-#include "go.h"
-#include "servo.h"
-#include "interrupt.h"
-#include "position.h"
-#include "controller.h"
-#include "interface.h"
-#include "coordinate.h"
-#include "check.h"
-int MoveToTarget(const double (&target_mm)[3], WorkState &state);
-///setup関連を集めた
-void Setups();
-void Tests();
-Interface in(USBTX, USBRX);
-int main()
-{
-    DEBUG("setup start\r\n");
-    Setups();
-    //スタート指示受付
-    //while(CanStart() == 0) {};
-    DEBUG("main start");
-    InterruptSetup();
-    WorkState state;
-    state.color = YELLOW;
-    state.areaname = WORKAREA;
-    GoToZeroSlowly();
-    
-
-    double posi[3] = {200,200,50};
-    SetNextRadRelative(0, -90*kDegreeToRad);
-    SetNextRadRelative(1, 150*kDegreeToRad);
-    SetNextRadRelative(3, 0*kDegreeToRad);
-    SetServoYawRad(0);
-    ServoMoveOnArm();
-    wait(1);
-    ServoMoveOnArm();
-    while(1) {
-        /*
-        while(in.AskPermission("change state",'y','n')) {
-            state.color = (Color)in.AskNum("color YELLOW : 0, RED : 1");
-            state.areaname = (AreaName) in.AskNum("areaname Work:0 common : 1 box :2 ");
-        }d
-        */
-        while(in.AskPermission("change target",'y','n')) {
-            for (int i = 0; i < 3; i++) posi[i]= in.AskNum("posi");
-        }
-        MoveToTarget(posi, state);
-        DEBUG("finish\r\n");
-        wait(0.5);
-        DEBUG("now x %f\r\n", GetNowTipLocateX());
-        DEBUG("now y %f\r\n", GetNowTipLocateY());
-        DEBUG("now z %f\r\n", GetNowTipLocateZ());
-    }
-}
-void Setups()
-{
-    GoSetup();
-    SetupPosition();
-
-}
-void Tests()
-{
-    static int count = 0;
-    count ++;
-    if (count > 100000) {
-        count =0;
-    }
-}
-
-#endif
\ No newline at end of file
+}
\ No newline at end of file