yei
Dependencies: interface mbed enc_1multi calPID motorout KondoServoLibrary
Fork of cat18_operate by
Diff: workinfo/workinfo.cpp
- Revision:
- 26:5e292422cc08
- Parent:
- 24:b4a89db4ed72
- Child:
- 28:bcfc84b481bc
--- a/workinfo/workinfo.cpp Mon Sep 03 05:40:59 2018 +0000 +++ b/workinfo/workinfo.cpp Mon Sep 03 07:10:21 2018 +0000 @@ -40,7 +40,7 @@ }; //workareaの補正値 const int kWorkAreaCorrect[kWorkAreaNum][3] = { - {0,0,0},{0,0,0},{0,0,0}, + {-70,0,0},{0,0,0},{0,0,0}, {0,0,0},{0,0,0},{0,0,0}, }; const int kCommonAreaCorrect[kCommonAreaNum][3] = { @@ -83,6 +83,75 @@ void ShootingBoxStuff(); void ExistReset();//is_existとcolorをリセット +void WorkPosition() +{ + DEBUG("WorkPosition() start\r\n"); +//ワークエリア + //x座標 + for(int i=0; i<3; i++) { + work[i].position[0] = -630 + 180 * i; + work[i + 3].position[0] = -540 + 180 * i; + } + //y座標 + for(int i=0; i<3; i++) { + work[i].position[1]=140; + work[i+3].position[1]=225+90; + } + //z座標 + for(int i=0; i<6; i++) work[i].position[2]=-436; +//共通ワークエリア + //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; + } else if(i<17) { + work[i].position[1]=675; + } else if(i< kCommonStart + kCommonAreaNum) { + work[i].position[1]=755; + } + } + //z座標 + for(int i=kCommonStart; i<kCommonStart + kCommonAreaNum; i++) work[i].position[2]=-400; + 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; + } + 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; + shootingbox[i*3+1].position[1]=160+80*i; + shootingbox[i*3+2].position[1]=160+80*i; + } + //z座標 + for(int i=0; i<sizeof(shootingbox)/sizeof(shootingbox[0]); i++) shootingbox[i].position[2]=100; + 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() { @@ -154,75 +223,6 @@ for(int i = kCommonStart; i < kCommonStart + kCommonAreaNum; i++) work[i].areaname = COMMONAREA; DEBUG("AreaNameSetup() finish\r\n"); } -void WorkPosition() -{ - DEBUG("WorkPosition() start\r\n"); -//ワークエリア - //x座標 - for(int i=0; i<3; i++) { - work[i].position[0] = -630 + 180 * i; - work[i + 3].position[0] = -540 + 180 * i; - } - //y座標 - for(int i=0; i<3; i++) { - work[i].position[1]=140; - work[i+3].position[1]=370; - } - //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; - } else if(i<17) { - work[i].position[1]=675; - } else if(i< kCommonStart + kCommonAreaNum) { - work[i].position[1]=755; - } - } - //z座標 - for(int i=kCommonStart; i<kCommonStart + kCommonAreaNum; i++) work[i].position[2]=-400; - 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; - } - 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; - shootingbox[i*3+1].position[1]=160+80*i; - shootingbox[i*3+2].position[1]=160+80*i; - } - //z座標 - for(int i=0; i<sizeof(shootingbox)/sizeof(shootingbox[0]); i++) shootingbox[i].position[2]=85; - 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 Priority() { DEBUG("Priority() start\r\n");