![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
yei
Dependencies: interface mbed enc_1multi calPID motorout KondoServoLibrary
Fork of cat18_operate by
workinfo/workinfo.cpp
- Committer:
- yuto17320508
- Date:
- 2018-09-09
- Revision:
- 45:51ab3ca47228
- Parent:
- 44:d02b2165c4ed
File content as of revision 45:51ab3ca47228:
#include "mbed.h" #include "state.h" #include "workinfo.h" #include "debug.h" #include "coordinate.h" //全部右で考えてる //穴の数 const int kWorkAreaNum = 6; const int kCommonAreaNum = 17; const int kBoxNum = 12; //ワークエリアの配列のいれかた int kWorkAreaIndex[kWorkAreaNum] = { 3, 4, 5, 0, 1, 2, }; //共通ワークエリア const int kCommonAreaIndex[kCommonAreaNum] = { 17, 18, 19, 20, 21, 22, 12, 13, 14, 15, 16, 6, 7, 8, 9, 10, 11, }; //shootingboxの配列のいれかた const int kBoxIndex[kBoxNum] = { 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0, }; //////////////////////////////変更出来るパラメータ //priority 取る順番を決める const int kWorkAreaPriority[kWorkAreaNum] = { 102, 101, 100, 105, 104, 103, }; const int kCommonAreaPriority[kCommonAreaNum] = { 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, }; //workareaの補正値 const int kWorkAreaCorrect[kWorkAreaNum][3] = { #ifdef RIGHT {0,0,-5},{0,10,-5},{-20,10,-3}, {0+20,0+30,0},{0,-20,0},{0,0,0}, #elif defined LEFT {0,-20,10},{0,0,10},{0,0,0}, {0+20,0+30,0},{0,-20,0},{0,0,0}, #endif }; const int kCommonAreaCorrect[kCommonAreaNum][3] = { #ifdef RIGHT {0,0,0},{0,0,0},{0,0,0},{-10,0,0},{-10,30,0},{-30,10,0}, {0,50,0},{0,50,0},{0,50,0},{0,50,0},{0,50,0}, {60,50,20},{0,50,0},{30,50,20},{20,50,0},{0,50,0},{-10,50,0}, #elif defined LEFT {0,20,0},{0,0,0},{0,40,0},{-10,50,0},{-10,30,0},{-30,50,0}, {0,50,0},{0,50,0},{0,50,0},{0,50,0},{0,50,0}, {10,50,0},{0,50,0},{30,50,0},{20,50,0},{0,50,0},{-10,50,0}, #endif }; const int kBoxCorrect[kBoxNum][3] = { #ifdef RIGHT {0,0,0},{0,0,0},{0,0,0}, {0,0,0},{0,0,0},{-20,30,0}, {0,0,0},{0,0,0},{-20,30,0}, {0,0,0},{0,0,0},{0,0,0}, #elif defined LEFT {0,0,0},{0,0,0},{0,0,0}, {0,0,0},{0,0,0},{-20,30,0}, {0,0,0},{0,0,0},{-20,30,0}, {0,0,0},{0,0,0},{0,0,0}, #endif }; const int kHandCenter = 22;//想定先端と実際の先端位置のずれ const int kYawWorkAreaRedDegree = 270; const int kYawWorkAreaYellowDegree = 90; const int kYawCommonAreaDegree = 90; const int kYawBoxDegree = 90; const int kPitchWorkAreaDegree = -90; const int kPitchCommonAreaDegree = -5; const int kPitchBoxDegree = 10; ////////////////////////////// //配列のスタート番号 const int kWorkStart = 0; const int kCommonStart = kWorkAreaNum; WorkPosi work[kWorkAreaNum + kCommonAreaNum + 1];//最後の1つは番兵 BoxPosi shootingbox[kBoxNum]; Pattern now_pattern = NOPATTERN; //関数名 void AreaNameSetup();//areanameを入れる。 void ShootingBoxPosition();//シューティングボックスにおける穴の座標を代入している void WorkPosition();//ワークエリア、共通ワークエリアにおける穴の座標を代入している void Priority();//ワークエリア、共通ワークエリアにおける穴の優先度を決めてる void ArrangementPattern(Pattern pattern); void ShootingBoxStuff(); void ExistReset();//is_existとcolorをリセット void WorkPosition() { DEBUG("WorkPosition() start\r\n"); //ワークエリア //x座標 for(int i=0; i<3; i++) { #ifdef RIGHT work[i].position[0] = -630 + 180 * i; work[i + 3].position[0] = -540 + 180 * i; #elif defined LEFT work[i].position[0] = -540 + 180 * i; work[i + 3].position[0] = -630 + 180 * i; #else DEBUG("error:There is no defined RIGHT/LEFT in WorkPosition\r\n"); #endif } //y座標 for(int i=0; i<3; i++) { work[i].position[1]= 300 - 150; work[i+3].position[1]= 210 + 150; } //z座標 for(int i=0; i<6; i++) work[i].position[2]=-420; //共通ワークエリア //x座標 for(int i = kCommonStart; i<12; i++) { work[i].position[0]=-595+(i-kCommonStart)*80; work[i+11].position[0]=-595+(i-kCommonStart)*80; } for(int i=12; i<17; i++) work[i].position[0]=-555+(i-12)*80; //y座標 for(int i=kCommonStart; i<sizeof(work)/sizeof(work[0]); i++) { if(i < 12) { work[i].position[1]=595 + kHandCenter; } else if(i < 17) { work[i].position[1]=675 + kHandCenter; } else if(i < kCommonStart + kCommonAreaNum) { work[i].position[1]=755 + kHandCenter; } } //z座標 for(int i=kCommonStart; i<kCommonStart + kCommonAreaNum; i++) work[i].position[2]=-350; for(int i = 0; i < kWorkAreaNum; i++) { for(int j = 0; j < 3; j++) work[kWorkAreaIndex[i]].position[j] += kWorkAreaCorrect[i][j]; } for(int i = 0; i < kCommonAreaNum; i++) { for(int j = 0; j < 3; j++) work[kCommonAreaIndex[i]].position[j] += kCommonAreaCorrect[i][j]; DEBUG("work %d %f,%f,%f\r\n",kCommonAreaIndex[i], work[kCommonAreaIndex[i]].position[0], work[kCommonAreaIndex[i]].position[1], work[kCommonAreaIndex[i]].position[2] ); } DEBUG("WorkPosition() finish\r\n"); } void ShootingBoxPosition() { DEBUG("ShootingBoxPosition() start\r\n"); //軸は一番右下の穴を原点に取っている. //x座標 for(int i=0; i<3; i++) { shootingbox[i].position[0]=240-80*i; shootingbox[i+3].position[0]=240-80*i; shootingbox[i+6].position[0]=240-80*i; shootingbox[i+9].position[0]=240-80*i; } //y座標 for(int i=0; i<4; i++) { shootingbox[i*3].position[1]=160+80*i; shootingbox[i*3+1].position[1]=160+80*i; shootingbox[i*3+2].position[1]=160+80*i; } for(int i = 0; i < kBoxNum; i++) shootingbox[i].position[1] += kArmLength[3] *(1 - cos(kPitchBoxDegree * kDegreeToRad)); //z座標 for(int i=0; i<sizeof(shootingbox)/sizeof(shootingbox[0]); i++) shootingbox[i].position[2]=125 + kArmLength[3] * sin(kPitchBoxDegree * kDegreeToRad); for(int i = 0; i < kBoxNum; i++) { for(int j = 0; j < 3; j++) shootingbox[kBoxIndex[i]].position[j] += kBoxCorrect[i][j]; } DEBUG("ShootingBoxPosition() finish\r\n"); } void SetupPosition() { DEBUG("SetupPosition() start\r\n"); ExistReset(); AreaNameSetup(); WorkPosition(); ShootingBoxPosition(); Priority(); DEBUG("SetupPosition() finish\r\n"); } double GetIdealYawRad(WorkState state) { double degree = 0; switch(state.areaname) { case BOX: degree = kYawBoxDegree; break; case COMMONAREA: degree = kYawCommonAreaDegree; break; case WORKAREA: switch(state.color) { case RED: degree = kYawWorkAreaRedDegree; break; case YELLOW: degree = kYawWorkAreaYellowDegree; break; } break; } return degree * kDegreeToRad; } double GetIdealPitchRad(WorkState state) { double degree = 0; switch(state.areaname) { case BOX: degree = kPitchBoxDegree; break; case COMMONAREA: degree = kPitchCommonAreaDegree; break; case WORKAREA: degree = kPitchWorkAreaDegree; break; } return degree * kDegreeToRad; } void YawPitchSetup() { DEBUG("YawPitchSetup() start\r\n"); for(int i = 0; i < kWorkAreaNum+ kCommonAreaNum; i++) { work[i].yaw_rad = GetIdealYawRad(work[i]); work[i].pitch_rad = GetIdealPitchRad(work[i]); } for(int i = 0; i < kBoxNum; i++) { shootingbox[i].yaw_rad = GetIdealYawRad(shootingbox[i]); shootingbox[i].pitch_rad = GetIdealPitchRad(shootingbox[i]); } DEBUG("YawPitchSetup() finish\r\n"); } //areanameをセット。BOXは初期段階で入っている。 void AreaNameSetup() { DEBUG("AreaNameSetup() start\r\n"); for(int i = kWorkStart; i < kWorkAreaNum; i++) work[i].areaname = WORKAREA; for(int i = kCommonStart; i < kCommonStart + kCommonAreaNum; i++) work[i].areaname = COMMONAREA; DEBUG("AreaNameSetup() finish\r\n"); } void Priority() { DEBUG("Priority() start\r\n"); work[23].priority=1000;//SearchCommonArea で使用。最初の時の比較に使用.最後だということを示す。 for(int i = kWorkStart; i < kWorkAreaNum; i++) work[kWorkAreaIndex[i]].priority = kWorkAreaPriority[i]; for(int i = 0; i < kCommonAreaNum; i++) work[kCommonAreaIndex[i]].priority = kCommonAreaPriority[i]; DEBUG("Priority() finish\r\n"); } void ArrangementPattern(Pattern pattern)//引数にはAとかで入力//初めに一回だけ呼び出すことを想定 { DEBUG("ArrangementPattern() start\r\n"); //workareaの設定 work[0].color=RED; work[1].color=RED; work[2].color=RED; work[3].color=YELLOW; work[4].color=YELLOW; work[5].color=YELLOW; switch(pattern) { case A: DEBUG("ArrangementPattern() A\r\n"); #ifdef RIGHT DEBUG("right\r\n"); work[6].color=RED; work[13].color=RED; work[20].color=RED; work[8].color=YELLOW; work[15].color=YELLOW; work[22].color=YELLOW; work[6].is_exist=1; work[13].is_exist=1; work[20].is_exist=1; work[8].is_exist=1; work[15].is_exist=1; work[22].is_exist=1; #elif defined LEFT DEBUG("left\r\n"); work[9].color=RED; work[13].color=RED; work[17].color=RED; work[11].color=YELLOW; work[15].color=YELLOW; work[19].color=YELLOW; work[9].is_exist=1; work[13].is_exist=1; work[17].is_exist=1; work[11].is_exist=1; work[15].is_exist=1; work[19].is_exist=1; #else DEBUG("error:There is no defined RIGHT/LEFT in ArrangementPattern\r\n"); #endif break; case B: DEBUG("ArrangementPattern() B\r\n"); #ifdef RIGHT DEBUG("right\r\n"); work[7].color=RED; work[9].color=RED; work[11].color=RED; work[17].color=YELLOW; work[19].color=YELLOW; work[21].color=YELLOW; work[7].is_exist=1; work[9].is_exist=1; work[11].is_exist=1; work[17].is_exist=1; work[19].is_exist=1; work[21].is_exist=1; #elif defined LEFT DEBUG("left\r\n"); work[18].color=RED; work[20].color=RED; work[22].color=RED; work[6].color=YELLOW; work[8].color=YELLOW; work[10].color=YELLOW; work[18].is_exist=1; work[20].is_exist=1; work[22].is_exist=1; work[6].is_exist=1; work[8].is_exist=1; work[10].is_exist=1; #else DEBUG("error:There is no defined RIGHT/LEFT in ArrangementPattern\r\n"); #endif break; case C: DEBUG("ArrangementPattern() C\r\n"); work[15].color=RED; work[22].color=RED; work[11].color=RED; work[19].color=YELLOW; work[12].color=YELLOW; work[8].color=YELLOW; work[15].is_exist=1; work[22].is_exist=1; work[11].is_exist=1; work[19].is_exist=1; work[12].is_exist=1; work[8].is_exist=1; break; case D: DEBUG("ArrangementPattern() D\r\n"); work[22].color=RED; work[15].color=RED; work[11].color=RED; work[17].color=YELLOW; work[13].color=YELLOW; work[6].color=YELLOW; work[22].is_exist=1; work[15].is_exist=1; work[11].is_exist=1; work[17].is_exist=1; work[13].is_exist=1; work[6].is_exist=1; break; default: break; } DEBUG("ArrangementPattern() finish\r\n"); } void ExistReset() { DEBUG("ExistReset() start\r\n"); ///初期化してる。いったん全部存在してないことにする//要はリセット for(int i=kCommonStart; i < kCommonStart + kCommonAreaNum; i++) { work[i].is_exist=0;//共通ワークエリアの初期化 work[i].color = NOCOLOR; } for(int i=kWorkStart; i < kWorkAreaNum; i++) { //初期化してる。ワークエリアの初期化 work[i].is_exist=1; } for(int i=0; i<12; i++) {//shootingの初期値 shootingbox[i].is_exist=0; shootingbox[i].color = NOCOLOR; } DEBUG("ExistReset() finish\r\n"); }