![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
yei
Dependencies: interface mbed enc_1multi calPID motorout KondoServoLibrary
Fork of cat18_operate by
Diff: workinfo/workinfo.cpp
- Revision:
- 37:d4b711619962
- Parent:
- 36:b8171eeefcd0
- Child:
- 38:9bed85d32c97
diff -r b8171eeefcd0 -r d4b711619962 workinfo/workinfo.cpp --- a/workinfo/workinfo.cpp Wed Sep 05 03:58:53 2018 +0000 +++ b/workinfo/workinfo.cpp Wed Sep 05 09:37:41 2018 +0000 @@ -39,19 +39,19 @@ }; //workareaの補正値 const int kWorkAreaCorrect[kWorkAreaNum][3] = { - {30,30,10},{0,20,10},{30,20,0}, - {0+20,0+30,100},{0,0,0},{0,0,0}, + {0,-20,10},{0,0,10},{30,0,10}, + {0+20,0+30,100},{0,-20,0},{0,0,0}, }; const int kCommonAreaCorrect[kCommonAreaNum][3] = { - {60,30,0},{0,50,0},{30,50+20,0},{0,50,0},{0,50,0},{-30,50,0}, - {0,50,0},{0,50,0},{0,50,0},{0,50,0},{0,50,0}, - {10,50,20},{0,50,20},{30,50,20},{80,50,0},{0,50,0},{0,30,0}, + {0,20,20},{0,0,0},{20,40,40},{-10,30,30},{-10,30,0},{-30,30,40}, + {0,50,0},{0,50,20},{0,50,0},{0,50,20},{0,50,0}, + {10,50,20},{0,50,20},{30,50,20},{20,50,0},{0,50,0},{-10,30,0}, }; const int kBoxCorrect[kBoxNum][3] = { - {-80,80,0},{-80,80,0},{-80,80,0}, - {-80,160,0},{-80,160,0},{-80,160,0}, - {-80,160,0},{-80,160,0},{-80,160,0}, - {-80,160,0},{-80,160,0},{-80,160,0}, +{0,0,0},{0,0,0},{0,0,0}, +{0,0,0},{0,0,0},{0,0,0}, +{0,0,0},{0,0,0},{0,0,0}, +{0,0,0},{0,0,0},{0,0,0}, }; const int kHandCenter = 22;//想定先端と実際の先端位置のずれ @@ -62,7 +62,7 @@ const int kPitchWorkAreaDegree = -90; const int kPitchCommonAreaDegree = -5; -const int kPitchBoxDegree = 10; +const int kPitchBoxDegree = 0; ////////////////////////////// //配列のスタート番号 const int kWorkStart = 0; @@ -144,14 +144,13 @@ shootingbox[i+6].position[0]=240-80*i; shootingbox[i+9].position[0]=240-80*i; } - for(int i = 0; i < kBoxNum; i++) shootingbox[i].position[0] += kHandCenter; //y座標 for(int i=0; i<4; i++) { - shootingbox[i*3].position[1]=160+80*i + kHandCenter; - shootingbox[i*3+1].position[1]=160+80*i + kHandCenter; - shootingbox[i*3+2].position[1]=160+80*i + kHandCenter; + 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)); + 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++) { @@ -170,7 +169,7 @@ Priority(); DEBUG("SetupPosition() finish\r\n"); } -double GetIdealYawRad(const WorkState &state) +double GetIdealYawRad(WorkState state) { double degree = 0; switch(state.areaname) { @@ -193,7 +192,7 @@ } return degree * kDegreeToRad; } -double GetIdealPitchRad(const WorkState &state) +double GetIdealPitchRad(WorkState state) { double degree = 0; switch(state.areaname) {