yuto kawamura / Mbed 2 deprecated cat18_operate_last

yei

Dependencies:   interface mbed enc_1multi calPID motorout KondoServoLibrary

Fork of cat18_operate by Catch the GIANT Caplico!

main.cpp@13:126c3f7f9b89, 2018-08-13 (annotated)

Committer:
shimizuta
Date:
Mon Aug 13 09:03:30 2018 +0000
Revision:
13:126c3f7f9b89
Parent:
10:6d1d44fa9739
Child:
16:b2358fd35999
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 0:c3a72fe24137 1 #include <mbed.h>
shimizuta 0:c3a72fe24137 2 #include "debug.h"//DEBUG("",変数);でデバッグ。
shimizuta 1:cc056c41ae30 3 #include "state.h"
shimizuta 1:cc056c41ae30 4 #include "go.h"
shimizuta 3:05b1dcb3634e 5 #include "servo.h"
shimizuta 2:4515e8cc6b60 6 #include "interrupt.h"
shimizuta 2:4515e8cc6b60 7 #include "position.h"
shimizuta 3:05b1dcb3634e 8 #include "controller.h"
shimizuta 10:6d1d44fa9739 9 //debug用に追加
shimizuta 8:cabc50cf3e1e 10 #include "interface.h"
shimizuta 10:6d1d44fa9739 11 #include "coordinate.h"
shimizuta 1:cc056c41ae30 12 ///setup関連を集めた
shimizuta 0:c3a72fe24137 13 void Setups();
shimizuta 0:c3a72fe24137 14 void Tests();
shimizuta 8:cabc50cf3e1e 15 Interface interface(USBTX, USBRX);
shimizuta 0:c3a72fe24137 16 int main()
shimizuta 0:c3a72fe24137 17 {
shimizuta 0:c3a72fe24137 18 DEBUG("setup start\r\n");
shimizuta 0:c3a72fe24137 19 Setups();
shimizuta 13:126c3f7f9b89 20 GoToZeroSlowly();
shimizuta 13:126c3f7f9b89 21
shimizuta 13:126c3f7f9b89 22
shimizuta 13:126c3f7f9b89 23 double posi[3] = {200,200,50};
shimizuta 13:126c3f7f9b89 24 SetNextRadRelative(0, -90*kDegreeToRad);
shimizuta 13:126c3f7f9b89 25 SetNextRadRelative(1, 150*kDegreeToRad);
shimizuta 13:126c3f7f9b89 26 SetNextRadRelative(3, 0*kDegreeToRad);
shimizuta 13:126c3f7f9b89 27 SetServoYawRad(0);
shimizuta 13:126c3f7f9b89 28 ServoMoveOnArm();
shimizuta 13:126c3f7f9b89 29 wait(1);
shimizuta 13:126c3f7f9b89 30 ServoMoveOnArm();
shimizuta 4:187c62291654 31 //スタート指示受付
shimizuta 10:6d1d44fa9739 32 while(CanStart() == 0) {};
shimizuta 0:c3a72fe24137 33 DEBUG("main start");
shimizuta 10:6d1d44fa9739 34 InterruptSetup();
shimizuta 4:187c62291654 35 int worknum = 0;
shimizuta 4:187c62291654 36 while( (worknum = CalPickPlace()) != 23 ) {
shimizuta 4:187c62291654 37 //ワークに向かう
shimizuta 2:4515e8cc6b60 38 if(Go(work[worknum]) != 0) continue;
shimizuta 13:126c3f7f9b89 39 DEBUG("finish 1\r\n");
shimizuta 4:187c62291654 40 //掴む
shimizuta 2:4515e8cc6b60 41 Close();
shimizuta 13:126c3f7f9b89 42 DEBUG("finish 2\r\n");
shimizuta 4:187c62291654 43 //上に持ち上げる
shimizuta 2:4515e8cc6b60 44 Above();
shimizuta 13:126c3f7f9b89 45 DEBUG("finish 3\r\n");
shimizuta 4:187c62291654 46 //取り上げたことを保存
shimizuta 2:4515e8cc6b60 47 work[worknum].is_exist = 0;
shimizuta 4:187c62291654 48 //シュート位置計算
shimizuta 4:187c62291654 49 int boxspace = CalPutPlace(work[worknum].color);
shimizuta 13:126c3f7f9b89 50 DEBUG("finish 4\r\n");
shimizuta 2:4515e8cc6b60 51 //ボックスに行く(失敗(-1)したらcontinue)
shimizuta 2:4515e8cc6b60 52 if(Go(shootingbox[boxspace]) != 0) continue;
shimizuta 13:126c3f7f9b89 53 DEBUG("finish 5\r\n");
shimizuta 4:187c62291654 54 //放す
shimizuta 2:4515e8cc6b60 55 Open();
shimizuta 13:126c3f7f9b89 56 DEBUG("finish 6\r\n");
shimizuta 4:187c62291654 57 //上に持ち上げる
shimizuta 2:4515e8cc6b60 58 Above();
shimizuta 13:126c3f7f9b89 59 DEBUG("finish 7\r\n");
shimizuta 4:187c62291654 60 //置いたことを保存
shimizuta 4:187c62291654 61 shootingbox[boxspace].is_exist = 1;
shimizuta 2:4515e8cc6b60 62 shootingbox[boxspace].color = work[worknum].color;
shimizuta 0:c3a72fe24137 63 }
shimizuta 0:c3a72fe24137 64 }
shimizuta 0:c3a72fe24137 65 void Setups()
shimizuta 0:c3a72fe24137 66 {
shimizuta 1:cc056c41ae30 67 GoSetup();
shimizuta 2:4515e8cc6b60 68 SetupPosition();
shimizuta 10:6d1d44fa9739 69
shimizuta 0:c3a72fe24137 70 }
shimizuta 0:c3a72fe24137 71 void Tests()
shimizuta 0:c3a72fe24137 72 {
shimizuta 0:c3a72fe24137 73 static int count = 0;
shimizuta 0:c3a72fe24137 74 count ++;
shimizuta 0:c3a72fe24137 75 if (count > 100000) {
shimizuta 0:c3a72fe24137 76 count =0;
shimizuta 0:c3a72fe24137 77 }
shimizuta 0:c3a72fe24137 78 }
shimizuta 1:cc056c41ae30 79
shimizuta 13:126c3f7f9b89 80 #if 0
shimizuta 13:126c3f7f9b89 81
shimizuta 13:126c3f7f9b89 82 #include <mbed.h>
shimizuta 13:126c3f7f9b89 83 #include "debug.h"//DEBUG("",変数);でデバッグ。
shimizuta 13:126c3f7f9b89 84 #include "state.h"
shimizuta 13:126c3f7f9b89 85 #include "go.h"
shimizuta 13:126c3f7f9b89 86 #include "servo.h"
shimizuta 13:126c3f7f9b89 87 #include "interrupt.h"
shimizuta 13:126c3f7f9b89 88 #include "position.h"
shimizuta 13:126c3f7f9b89 89 #include "controller.h"
shimizuta 13:126c3f7f9b89 90 #include "interface.h"
shimizuta 13:126c3f7f9b89 91 #include "coordinate.h"
shimizuta 13:126c3f7f9b89 92 #include "check.h"
shimizuta 13:126c3f7f9b89 93 int MoveToTarget(const double (&target_mm)[3], WorkState &state);
shimizuta 13:126c3f7f9b89 94 ///setup関連を集めた
shimizuta 13:126c3f7f9b89 95 void Setups();
shimizuta 13:126c3f7f9b89 96 void Tests();
shimizuta 13:126c3f7f9b89 97 Interface in(USBTX, USBRX);
shimizuta 13:126c3f7f9b89 98 int main()
shimizuta 13:126c3f7f9b89 99 {
shimizuta 13:126c3f7f9b89 100 DEBUG("setup start\r\n");
shimizuta 13:126c3f7f9b89 101 Setups();
shimizuta 13:126c3f7f9b89 102 //スタート指示受付
shimizuta 13:126c3f7f9b89 103 //while(CanStart() == 0) {};
shimizuta 13:126c3f7f9b89 104 DEBUG("main start");
shimizuta 13:126c3f7f9b89 105 InterruptSetup();
shimizuta 13:126c3f7f9b89 106 WorkState state;
shimizuta 13:126c3f7f9b89 107 state.color = YELLOW;
shimizuta 13:126c3f7f9b89 108 state.areaname = WORKAREA;
shimizuta 13:126c3f7f9b89 109 GoToZeroSlowly();
shimizuta 13:126c3f7f9b89 110
shimizuta 13:126c3f7f9b89 111
shimizuta 13:126c3f7f9b89 112 double posi[3] = {200,200,50};
shimizuta 13:126c3f7f9b89 113 SetNextRadRelative(0, -90*kDegreeToRad);
shimizuta 13:126c3f7f9b89 114 SetNextRadRelative(1, 150*kDegreeToRad);
shimizuta 13:126c3f7f9b89 115 SetNextRadRelative(3, 0*kDegreeToRad);
shimizuta 13:126c3f7f9b89 116 SetServoYawRad(0);
shimizuta 13:126c3f7f9b89 117 ServoMoveOnArm();
shimizuta 13:126c3f7f9b89 118 wait(1);
shimizuta 13:126c3f7f9b89 119 ServoMoveOnArm();
shimizuta 13:126c3f7f9b89 120 while(1) {
shimizuta 13:126c3f7f9b89 121 /*
shimizuta 13:126c3f7f9b89 122 while(in.AskPermission("change state",'y','n')) {
shimizuta 13:126c3f7f9b89 123 state.color = (Color)in.AskNum("color YELLOW : 0, RED : 1");
shimizuta 13:126c3f7f9b89 124 state.areaname = (AreaName) in.AskNum("areaname Work:0 common : 1 box :2 ");
shimizuta 13:126c3f7f9b89 125 }d
shimizuta 13:126c3f7f9b89 126 */
shimizuta 13:126c3f7f9b89 127 while(in.AskPermission("change target",'y','n')) {
shimizuta 13:126c3f7f9b89 128 for (int i = 0; i < 3; i++) posi[i]= in.AskNum("posi");
shimizuta 13:126c3f7f9b89 129 }
shimizuta 13:126c3f7f9b89 130 MoveToTarget(posi, state);
shimizuta 13:126c3f7f9b89 131 DEBUG("finish\r\n");
shimizuta 13:126c3f7f9b89 132 wait(0.5);
shimizuta 13:126c3f7f9b89 133 DEBUG("now x %f\r\n", GetNowTipLocateX());
shimizuta 13:126c3f7f9b89 134 DEBUG("now y %f\r\n", GetNowTipLocateY());
shimizuta 13:126c3f7f9b89 135 DEBUG("now z %f\r\n", GetNowTipLocateZ());
shimizuta 13:126c3f7f9b89 136 }
shimizuta 13:126c3f7f9b89 137 }
shimizuta 13:126c3f7f9b89 138 void Setups()
shimizuta 13:126c3f7f9b89 139 {
shimizuta 13:126c3f7f9b89 140 GoSetup();
shimizuta 13:126c3f7f9b89 141 SetupPosition();
shimizuta 13:126c3f7f9b89 142
shimizuta 13:126c3f7f9b89 143 }
shimizuta 13:126c3f7f9b89 144 void Tests()
shimizuta 13:126c3f7f9b89 145 {
shimizuta 13:126c3f7f9b89 146 static int count = 0;
shimizuta 13:126c3f7f9b89 147 count ++;
shimizuta 13:126c3f7f9b89 148 if (count > 100000) {
shimizuta 13:126c3f7f9b89 149 count =0;
shimizuta 13:126c3f7f9b89 150 }
shimizuta 13:126c3f7f9b89 151 }
shimizuta 13:126c3f7f9b89 152
shimizuta 13:126c3f7f9b89 153 #endif