Diff: main.cpp

Revision:

13:126c3f7f9b89

Parent:

10:6d1d44fa9739

Child:

16:b2358fd35999

--- a/main.cpp	Mon Aug 13 02:22:58 2018 +0000
+++ b/main.cpp	Mon Aug 13 09:03:30 2018 +0000
@@ -17,6 +17,17 @@
 {
     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");
@@ -25,20 +36,27 @@
     while( (worknum = CalPickPlace()) != 23 ) {
         //ワークに向かう
         if(Go(work[worknum]) != 0) continue;
+        DEBUG("finish 1\r\n");
         //掴む
         Close();
+        DEBUG("finish 2\r\n");
         //上に持ち上げる
         Above();
+        DEBUG("finish 3\r\n");
         //取り上げたことを保存
         work[worknum].is_exist = 0;
         //シュート位置計算
         int boxspace = CalPutPlace(work[worknum].color);
+        DEBUG("finish 4\r\n");
         //ボックスに行く(失敗(-1)したらcontinue)
         if(Go(shootingbox[boxspace]) != 0) continue;
+        DEBUG("finish 5\r\n");
         //放す
         Open();
+        DEBUG("finish 6\r\n");
         //上に持ち上げる
         Above();
+        DEBUG("finish 7\r\n");
         //置いたことを保存
         shootingbox[boxspace].is_exist = 1;
         shootingbox[boxspace].color = work[worknum].color;
@@ -59,3 +77,77 @@
     }
 }
 
+#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