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@18:05f5a3323bda, 2018-08-17 (annotated)

Committer:
shimizuta
Date:
Fri Aug 17 00:10:25 2018 +0000
Revision:
18:05f5a3323bda
Parent:
16:b2358fd35999
Child:
19:48c3af917932
changed AllowChangeDirection

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 16:b2358fd35999 7 #include "workposition.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 18:05f5a3323bda 12
shimizuta 18:05f5a3323bda 13 const int kWait_ms = 500;
shimizuta 18:05f5a3323bda 14
shimizuta 18:05f5a3323bda 15 //#define FORDEBUG
shimizuta 1:cc056c41ae30 16 ///setup関連を集めた
shimizuta 0:c3a72fe24137 17 void Setups();
shimizuta 0:c3a72fe24137 18 void Tests();
shimizuta 8:cabc50cf3e1e 19 Interface interface(USBTX, USBRX);
shimizuta 0:c3a72fe24137 20 int main()
shimizuta 0:c3a72fe24137 21 {
shimizuta 0:c3a72fe24137 22 DEBUG("setup start\r\n");
shimizuta 0:c3a72fe24137 23 Setups();
shimizuta 13:126c3f7f9b89 24
shimizuta 4:187c62291654 25 //スタート指示受付
shimizuta 10:6d1d44fa9739 26 while(CanStart() == 0) {};
shimizuta 0:c3a72fe24137 27 DEBUG("main start");
shimizuta 10:6d1d44fa9739 28 InterruptSetup();
shimizuta 4:187c62291654 29 int worknum = 0;
shimizuta 18:05f5a3323bda 30
shimizuta 18:05f5a3323bda 31 #ifdef FORDEBUG
shimizuta 18:05f5a3323bda 32 for(int i = kCommonStart; i < kCommonStart + kCommonAreaNum - 2; i++) work[i].is_exist = 0;
shimizuta 18:05f5a3323bda 33 #endif
shimizuta 4:187c62291654 34 while( (worknum = CalPickPlace()) != 23 ) {
shimizuta 18:05f5a3323bda 35 Close();
shimizuta 4:187c62291654 36 //ワークに向かう
shimizuta 16:b2358fd35999 37 DEBUG("worknum %d, color %d, area %d, position %f, %f, %f\r\n", worknum, work[worknum].color, work[worknum].areaname, work[worknum].position[0],work[worknum].position[1],work[worknum].position[2]);
shimizuta 16:b2358fd35999 38 DEBUG("go to work\r\n");
shimizuta 2:4515e8cc6b60 39 if(Go(work[worknum]) != 0) continue;
shimizuta 16:b2358fd35999 40 DEBUG("close\r\n");
shimizuta 4:187c62291654 41 //掴む
shimizuta 2:4515e8cc6b60 42 Close();
shimizuta 16:b2358fd35999 43 DEBUG("above\r\n");
shimizuta 18:05f5a3323bda 44 wait_ms(kWait_ms);
shimizuta 4:187c62291654 45 //上に持ち上げる
shimizuta 18:05f5a3323bda 46 Above(work[worknum].areaname);
shimizuta 18:05f5a3323bda 47 wait_ms(kWait_ms);
shimizuta 4:187c62291654 48 //取り上げたことを保存
shimizuta 2:4515e8cc6b60 49 work[worknum].is_exist = 0;
shimizuta 4:187c62291654 50 //シュート位置計算
shimizuta 16:b2358fd35999 51 DEBUG("cal put place\r\n");
shimizuta 4:187c62291654 52 int boxspace = CalPutPlace(work[worknum].color);
shimizuta 2:4515e8cc6b60 53 //ボックスに行く(失敗(-1)したらcontinue)
shimizuta 16:b2358fd35999 54 DEBUG("go to box %d\r\n",boxspace);
shimizuta 2:4515e8cc6b60 55 if(Go(shootingbox[boxspace]) != 0) continue;
shimizuta 16:b2358fd35999 56 DEBUG("open\r\n");
shimizuta 4:187c62291654 57 //放す
shimizuta 2:4515e8cc6b60 58 Open();
shimizuta 18:05f5a3323bda 59 wait_ms(kWait_ms);
shimizuta 16:b2358fd35999 60 DEBUG("above\r\n");
shimizuta 4:187c62291654 61 //上に持ち上げる
shimizuta 18:05f5a3323bda 62 Above(BOX);
shimizuta 18:05f5a3323bda 63 wait_ms(kWait_ms);
shimizuta 16:b2358fd35999 64 DEBUG("finish\r\n");
shimizuta 4:187c62291654 65 //置いたことを保存
shimizuta 4:187c62291654 66 shootingbox[boxspace].is_exist = 1;
shimizuta 2:4515e8cc6b60 67 shootingbox[boxspace].color = work[worknum].color;
shimizuta 0:c3a72fe24137 68 }
shimizuta 0:c3a72fe24137 69 }
shimizuta 0:c3a72fe24137 70 void Setups()
shimizuta 0:c3a72fe24137 71 {
shimizuta 1:cc056c41ae30 72 GoSetup();
shimizuta 2:4515e8cc6b60 73 SetupPosition();
shimizuta 18:05f5a3323bda 74 StartCheck();
shimizuta 0:c3a72fe24137 75 }
shimizuta 0:c3a72fe24137 76 void Tests()
shimizuta 0:c3a72fe24137 77 {
shimizuta 0:c3a72fe24137 78 static int count = 0;
shimizuta 0:c3a72fe24137 79 count ++;
shimizuta 0:c3a72fe24137 80 if (count > 100000) {
shimizuta 18:05f5a3323bda 81
shimizuta 0:c3a72fe24137 82 count =0;
shimizuta 0:c3a72fe24137 83 }
shimizuta 16:b2358fd35999 84 }