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