yei
Dependencies: interface mbed enc_1multi calPID motorout KondoServoLibrary
Fork of cat18_operate by
main.cpp
- Committer:
- yuto17320508
- Date:
- 2018-09-08
- Revision:
- 41:7e3b95c2fcac
- Parent:
- 40:583e65cf4fd2
File content as of revision 41:7e3b95c2fcac:
#include <mbed.h> #include "debug.h"//DEBUG("",変数);でデバッグ。 #include "state.h" #include "go.h" #include "servo.h" #include "interrupt.h" #include "workinfo.h" #include "controller.h" #include "calplace.h" #include "check.h" #include "discoveryboard.h" //debug用に追加 #include "coordinate.h" //#define DEBUG_MODE const int kWait_ms = 500; const double kServoOffsetDegreeInBox = #ifdef RIGHT 7; #elif defined LEFT 7; #endif int worknum = 0; int main() { GoSetup();//encoder等のsetup SetupPosition();//ワークの位置設定 if(PositionSetupIsInBan() == 1) return 1;//ワーク位置が禁止領域に入っていたら強制終了 GoToFirstPosition();//機体をスタート位置に持っていく DEBUG("stand by. Please put pattern button.\r\n"); while(PermitStart() == 0) {};//スタート指示受付 DEBUG("main start\r\n"); InterruptSetup(); while((worknum = CalPickPlace()) != 23 ) {//取るワーク計算.23は番兵 //ワークに向かう.-1でスキップ if(Go(work[worknum]) == -1) { work[worknum].is_exist = 0; continue; } wait_ms(kWait_ms); //掴む Close(); wait_ms(kWait_ms); //上に持ち上げる Above(work[worknum].areaname); wait_ms(kWait_ms); //取り上げたことを保存 work[worknum].is_exist = 0; //シュート位置計算 int boxspace = CalPutPlace(work[worknum].color); OriginDegree[0] = kOriginDegree[0] - kServoOffsetDegreeInBox; //ボックスに行く(失敗(-1)したらcontinue) //3のときだけ特別に速度下げる if(worknum == 0) servo_dist_mm /= 2; if(Go(shootingbox[boxspace]) != 0) continue; servo_dist_mm = kServoVelocity_mm* kCalTargetTicker_s ;// ManualPutPlace(shootingbox[boxspace], 15); //放す Open(); wait_ms(kWait_ms); OriginDegree[0] = kOriginDegree[0]; //上に持ち上げる Above(BOX); wait_ms(kWait_ms); //置いたことを保存 shootingbox[boxspace].is_exist = 1; shootingbox[boxspace].color = work[worknum].color; } // SpiSetup(); //ManualMode(); }