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@23:f45eb02433a5, 2018-09-02 (annotated)

Committer:
shimizuta
Date:
Sun Sep 02 07:53:06 2018 +0000
Revision:
23:f45eb02433a5
Parent:
20:13934809e117
Child:
24:b4a89db4ed72
can move with 3 servo

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 10:6d1d44fa9739 11 //debug用に追加
shimizuta 20:13934809e117 12 //#include "interface.h"
shimizuta 10:6d1d44fa9739 13 #include "coordinate.h"
shimizuta 20:13934809e117 14 //#define FORDEBUG
shimizuta 18:05f5a3323bda 15 const int kWait_ms = 500;
shimizuta 20:13934809e117 16 int worknum = 0;
shimizuta 23:f45eb02433a5 17 ///setup関連を集めた.成功:0, エラー:-1
shimizuta 23:f45eb02433a5 18 int Setups();
shimizuta 0:c3a72fe24137 19 void Tests();
shimizuta 20:13934809e117 20 //Interface interface(USBTX, USBRX);
shimizuta 0:c3a72fe24137 21 int main()
shimizuta 0:c3a72fe24137 22 {
shimizuta 23:f45eb02433a5 23 if(Setups() == -1) return 1;
shimizuta 4:187c62291654 24 //スタート指示受付
shimizuta 20:13934809e117 25 DEBUG("stand by\r\n");
shimizuta 23:f45eb02433a5 26 if(WasLoad() == 0) {
shimizuta 23:f45eb02433a5 27 wait(1);
shimizuta 23:f45eb02433a5 28 while(CanStart() == 0) {};
shimizuta 23:f45eb02433a5 29 }
shimizuta 20:13934809e117 30 DEBUG("main start\r\n");
shimizuta 10:6d1d44fa9739 31 InterruptSetup();
shimizuta 23:f45eb02433a5 32 ControllerSetup();
shimizuta 4:187c62291654 33 while( (worknum = CalPickPlace()) != 23 ) {
shimizuta 18:05f5a3323bda 34 Close();
shimizuta 4:187c62291654 35 //ワークに向かう
shimizuta 2:4515e8cc6b60 36 if(Go(work[worknum]) != 0) continue;
shimizuta 4:187c62291654 37 //掴む
shimizuta 2:4515e8cc6b60 38 Close();
shimizuta 18:05f5a3323bda 39 wait_ms(kWait_ms);
shimizuta 4:187c62291654 40 //上に持ち上げる
shimizuta 18:05f5a3323bda 41 Above(work[worknum].areaname);
shimizuta 18:05f5a3323bda 42 wait_ms(kWait_ms);
shimizuta 4:187c62291654 43 //取り上げたことを保存
shimizuta 2:4515e8cc6b60 44 work[worknum].is_exist = 0;
shimizuta 4:187c62291654 45 //シュート位置計算
shimizuta 4:187c62291654 46 int boxspace = CalPutPlace(work[worknum].color);
shimizuta 2:4515e8cc6b60 47 //ボックスに行く(失敗(-1)したらcontinue)
shimizuta 2:4515e8cc6b60 48 if(Go(shootingbox[boxspace]) != 0) continue;
shimizuta 4:187c62291654 49 //放す
shimizuta 2:4515e8cc6b60 50 Open();
shimizuta 18:05f5a3323bda 51 wait_ms(kWait_ms);
shimizuta 4:187c62291654 52 //上に持ち上げる
shimizuta 18:05f5a3323bda 53 Above(BOX);
shimizuta 18:05f5a3323bda 54 wait_ms(kWait_ms);
shimizuta 4:187c62291654 55 //置いたことを保存
shimizuta 4:187c62291654 56 shootingbox[boxspace].is_exist = 1;
shimizuta 2:4515e8cc6b60 57 shootingbox[boxspace].color = work[worknum].color;
shimizuta 0:c3a72fe24137 58 }
shimizuta 0:c3a72fe24137 59 }
shimizuta 23:f45eb02433a5 60 int Setups()
shimizuta 0:c3a72fe24137 61 {
shimizuta 23:f45eb02433a5 62 int is_error = 0;
shimizuta 1:cc056c41ae30 63 GoSetup();
shimizuta 2:4515e8cc6b60 64 SetupPosition();
shimizuta 23:f45eb02433a5 65 if(PositionSetupIsInBan() == 1) is_error = -1;
shimizuta 18:05f5a3323bda 66 StartCheck();
shimizuta 23:f45eb02433a5 67 return is_error;
shimizuta 23:f45eb02433a5 68
shimizuta 0:c3a72fe24137 69 }
shimizuta 0:c3a72fe24137 70 void Tests()
shimizuta 0:c3a72fe24137 71 {
shimizuta 0:c3a72fe24137 72 static int count = 0;
shimizuta 0:c3a72fe24137 73 count ++;
shimizuta 0:c3a72fe24137 74 if (count > 100000) {
shimizuta 0:c3a72fe24137 75 count =0;
shimizuta 0:c3a72fe24137 76 }
shimizuta 20:13934809e117 77 }