![](/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@38:9bed85d32c97, 2018-09-06 (annotated)
- Committer:
- shimizuta
- Date:
- Thu Sep 06 03:37:20 2018 +0000
- Revision:
- 38:9bed85d32c97
- Parent:
- 37:d4b711619962
- Child:
- 39:ac2d5ae81fc1
a
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 | 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 | 37:d4b711619962 | 15 | const int kWait_ms = 500; |
shimizuta | 38:9bed85d32c97 | 16 | const double kServoOffsetDegreeInBox = 5; |
shimizuta | 37:d4b711619962 | 17 | |
shimizuta | 20:13934809e117 | 18 | int worknum = 0; |
shimizuta | 0:c3a72fe24137 | 19 | int main() |
shimizuta | 0:c3a72fe24137 | 20 | { |
shimizuta | 38:9bed85d32c97 | 21 | GoSetup();//encoder等のsetup |
shimizuta | 38:9bed85d32c97 | 22 | SetupPosition();//ワークの位置設定 |
shimizuta | 38:9bed85d32c97 | 23 | if(PositionSetupIsInBan() == 1) return 1;//ワーク位置が禁止領域に入っていたら強制終了 |
shimizuta | 38:9bed85d32c97 | 24 | GoToFirstPosition();//機体をスタート位置に持っていく |
shimizuta | 38:9bed85d32c97 | 25 | DEBUG("stand by. Please put pattern button.\r\n"); |
shimizuta | 38:9bed85d32c97 | 26 | while(PermitStart() == 0) {};//スタート指示受付 |
shimizuta | 20:13934809e117 | 27 | DEBUG("main start\r\n"); |
shimizuta | 10:6d1d44fa9739 | 28 | InterruptSetup(); |
shimizuta | 38:9bed85d32c97 | 29 | while((worknum = CalPickPlace()) != 23 ) {//取るワーク計算.23は番兵 |
shimizuta | 38:9bed85d32c97 | 30 | //ワークに向かう.-1でスキップ |
shimizuta | 37:d4b711619962 | 31 | if(Go(work[worknum]) == -1) { |
shimizuta | 37:d4b711619962 | 32 | work[worknum].is_exist = 0; |
shimizuta | 37:d4b711619962 | 33 | continue; |
shimizuta | 37:d4b711619962 | 34 | } |
shimizuta | 38:9bed85d32c97 | 35 | wait_ms(kWait_ms); |
shimizuta | 4:187c62291654 | 36 | //掴む |
shimizuta | 2:4515e8cc6b60 | 37 | Close(); |
shimizuta | 18:05f5a3323bda | 38 | wait_ms(kWait_ms); |
shimizuta | 4:187c62291654 | 39 | //上に持ち上げる |
shimizuta | 18:05f5a3323bda | 40 | Above(work[worknum].areaname); |
shimizuta | 18:05f5a3323bda | 41 | wait_ms(kWait_ms); |
shimizuta | 4:187c62291654 | 42 | //取り上げたことを保存 |
shimizuta | 2:4515e8cc6b60 | 43 | work[worknum].is_exist = 0; |
shimizuta | 4:187c62291654 | 44 | //シュート位置計算 |
shimizuta | 4:187c62291654 | 45 | int boxspace = CalPutPlace(work[worknum].color); |
shimizuta | 37:d4b711619962 | 46 | OriginDegree[0] = kOriginDegree[0] - kServoOffsetDegreeInBox; |
shimizuta | 2:4515e8cc6b60 | 47 | //ボックスに行く(失敗(-1)したらcontinue) |
shimizuta | 31:f7eb7c9fc8db | 48 | //3のときだけ特別に速度下げる |
shimizuta | 38:9bed85d32c97 | 49 | if(worknum == 0) servo_dist_mm = 200 * kCalTargetTicker_s; |
shimizuta | 2:4515e8cc6b60 | 50 | if(Go(shootingbox[boxspace]) != 0) continue; |
shimizuta | 38:9bed85d32c97 | 51 | servo_dist_mm = kServoVelocity_mm* kCalTargetTicker_s ;// |
shimizuta | 38:9bed85d32c97 | 52 | ManualPutPlace(shootingbox[boxspace], 15); |
shimizuta | 4:187c62291654 | 53 | //放す |
shimizuta | 2:4515e8cc6b60 | 54 | Open(); |
shimizuta | 37:d4b711619962 | 55 | wait_ms(kWait_ms); |
shimizuta | 37:d4b711619962 | 56 | OriginDegree[0] = kOriginDegree[0]; |
shimizuta | 4:187c62291654 | 57 | //上に持ち上げる |
shimizuta | 18:05f5a3323bda | 58 | Above(BOX); |
shimizuta | 18:05f5a3323bda | 59 | wait_ms(kWait_ms); |
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 | 24:b4a89db4ed72 | 64 | } |