![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
yei
Dependencies: interface mbed enc_1multi calPID motorout KondoServoLibrary
Fork of cat18_operate by
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