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@41:7e3b95c2fcac, 2018-09-08 (annotated)

Committer:
yuto17320508
Date:
Sat Sep 08 04:35:17 2018 +0000
Revision:
41:7e3b95c2fcac
Parent:
40:583e65cf4fd2 
a

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