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@30:af136b83b202, 2018-09-04 (annotated)

Committer:
shimizuta
Date:
Tue Sep 04 23:03:50 2018 +0000
Revision:
30:af136b83b202
Parent:
29:ea48140fc85e
Child:
31:f7eb7c9fc8db
aaaaaa

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 20:13934809e117 7 #include "workinfo.h"
shimizuta 3:05b1dcb3634e 8 #include "controller.h"
shimizuta 20:13934809e117 9 #include "calplace.h"
shimizuta 23:f45eb02433a5 10 #include "check.h"
shimizuta 30:af136b83b202 11 #include "discoveryboard.h"
shimizuta 10:6d1d44fa9739 12 //debug用に追加
shimizuta 10:6d1d44fa9739 13 #include "coordinate.h"
shimizuta 30:af136b83b202 14 //#define DEBUG_MODE
shimizuta 30:af136b83b202 15 const int kWait_ms = 250;
shimizuta 20:13934809e117 16 int worknum = 0;
shimizuta 0:c3a72fe24137 17 int main()
shimizuta 0:c3a72fe24137 18 {
shimizuta 30:af136b83b202 19
shimizuta 24:b4a89db4ed72 20 GoSetup();
shimizuta 29:ea48140fc85e 21 GoToFirstPosition();
shimizuta 24:b4a89db4ed72 22 SetupPosition();
shimizuta 24:b4a89db4ed72 23 if(PositionSetupIsInBan() == 1) return 1;//失敗したら強制終了
shimizuta 30:af136b83b202 24
shimizuta 4:187c62291654 25 //スタート指示受付
shimizuta 20:13934809e117 26 DEBUG("stand by\r\n");
shimizuta 23:f45eb02433a5 27 if(WasLoad() == 0) {
shimizuta 23:f45eb02433a5 28 wait(1);
shimizuta 23:f45eb02433a5 29 while(CanStart() == 0) {};
shimizuta 23:f45eb02433a5 30 }
shimizuta 20:13934809e117 31 DEBUG("main start\r\n");
shimizuta 10:6d1d44fa9739 32 InterruptSetup();
shimizuta 23:f45eb02433a5 33 ControllerSetup();
shimizuta 24:b4a89db4ed72 34 while((worknum = CalPickPlace()) != 23 ) {
shimizuta 18:05f5a3323bda 35 Close();
shimizuta 4:187c62291654 36 //ワークに向かう
shimizuta 2:4515e8cc6b60 37 if(Go(work[worknum]) != 0) continue;
shimizuta 30:af136b83b202 38
shimizuta 30:af136b83b202 39 #ifdef DEBUG_MODE
shimizuta 29:ea48140fc85e 40 DEBUG("ManualModeWithPc start\r\n");
shimizuta 30:af136b83b202 41 while(ManualModeWithPc() == 1) {};
shimizuta 29:ea48140fc85e 42 DEBUG("ManualModeWithPc return work %d %f, %f, %f\r\n", worknum, GetNowTipLocateX(), GetNowTipLocateY(), GetNowTipLocateZ());
shimizuta 30:af136b83b202 43 #endif
shimizuta 4:187c62291654 44 //掴む
yoka06 26:5e292422cc08 45 wait_ms(kWait_ms);
shimizuta 2:4515e8cc6b60 46 Close();
shimizuta 18:05f5a3323bda 47 wait_ms(kWait_ms);
shimizuta 4:187c62291654 48 //上に持ち上げる
shimizuta 18:05f5a3323bda 49 Above(work[worknum].areaname);
shimizuta 18:05f5a3323bda 50 wait_ms(kWait_ms);
shimizuta 4:187c62291654 51 //取り上げたことを保存
shimizuta 2:4515e8cc6b60 52 work[worknum].is_exist = 0;
shimizuta 4:187c62291654 53 //シュート位置計算
shimizuta 4:187c62291654 54 int boxspace = CalPutPlace(work[worknum].color);
shimizuta 2:4515e8cc6b60 55 //ボックスに行く(失敗(-1)したらcontinue)
shimizuta 2:4515e8cc6b60 56 if(Go(shootingbox[boxspace]) != 0) continue;
shimizuta 4:187c62291654 57 //放す
shimizuta 2:4515e8cc6b60 58 Open();
shimizuta 4:187c62291654 59 //上に持ち上げる
shimizuta 18:05f5a3323bda 60 Above(BOX);
shimizuta 18:05f5a3323bda 61 wait_ms(kWait_ms);
shimizuta 4:187c62291654 62 //置いたことを保存
shimizuta 4:187c62291654 63 shootingbox[boxspace].is_exist = 1;
shimizuta 2:4515e8cc6b60 64 shootingbox[boxspace].color = work[worknum].color;
shimizuta 0:c3a72fe24137 65 }
shimizuta 24:b4a89db4ed72 66 }