NHK2022Aチームの足回りと機構のセット メインプログラム
Dependencies: FEP_RX22 OmniPosition PID R1370 SerialMultiByte Servo ikarashiMDC_2byte_ver omni_wheel
main.cpp
- Committer:
- me33004m
- Date:
- 2022-10-08
- Revision:
- 1:5132f966db08
- Parent:
- 0:5d4705b2893c
- Child:
- 2:d84346eaa720
File content as of revision 1:5132f966db08:
#include "mbed.h" #include "ikarashiMDC.h" #include "controller.h" #include "pinconfig.h" #include "omni_wheel.h" #include "math.h" #include "OmniPosition.h" #include "PID.h" #define PI 3.141592653589 Bcon mycon(fepTX, fepRX, fepad); Serial pc(pcTX, pcRX, 115200); Serial RS485(PC_10,PC_11,115200); uint8_t b[8]; int16_t stick[4]; double engine[4]; double speed[6]; double spin_power; double posiX , posiY , posiTheta; DigitalOut stop(A3); ikarashiMDC motor[] = { ikarashiMDC(0,0,SM,&RS485), ikarashiMDC(0,1,SM,&RS485), ikarashiMDC(0,2,SM,&RS485), ikarashiMDC(0,3,SM,&RS485), }; OmniWheel omni(4); OmniPosition posi(mwTX, mwRX); PID angle(10.0, 5.0, 0.0000005, 0.01);//値はhttps://controlabo.com/pid-gain/で調整しよう! /*プロトタイプ宣言*/ void chassis();//足回りの制御・ジャイロ・テラターム void spin(double ang);//PID int main(void){ mycon.StartReceive(); omni.wheel[0].setRadian(PI * 1.0 / 4.0); omni.wheel[1].setRadian(PI * 3.0 / 4.0); omni.wheel[2].setRadian(PI * 5.0 / 4.0); omni.wheel[3].setRadian(PI * 7.0 / 4.0); angle.setInputLimits(-180,180); angle.setOutputLimits(-0.4,0.4); angle.setBias(0); angle.setMode(1); angle.setSetPoint(0); while(1){ stop = 1; chassis(); } } void chassis(){ /*非常停止*/ //stop = 1; /*ジャイロ読み取り*/ posiX = posi.getX(); posiY = posi.getY(); posiTheta = posi.getTheta()*(180.0/M_PI);//OmniPositionライブラリが弧度法で角度をつけていたため60分法に変換 pc.printf("x:%5.2f Y:%5.2f theta:%0.3f | ",posiX, posiY,posiTheta); /*コントローラー受信*/ for (int i=0; i<8; i++) b[i] = mycon.getButton(i); for (int i=0; i<4; i++) stick[i] = mycon.getStick(i); for (int i=0; i<8; i++) pc.printf("%d ", b[i]); pc.printf(" | "); for (int i=0; i<4; i++) pc.printf("%3d ", stick[i]); pc.printf(" | "); pc.printf("spinning... %.2f|%.3f",posiTheta,-angle.compute()); if (mycon.status) pc.printf("received\r\n"); else{ pc.printf("anything error...\r\n"); for( int i=0; i<4; i++){ motor[i].setSpeed(0); } } /*全方位*/ for(int i=0; i<4; i++){ if(abs(stick[i]) > 10){ engine[i] = 0.4*(stick[i]/128.0); }else{ engine[i] = 0; } } /*PID*/ /*スパゲッティコードで申し訳ないです...*/ if(abs(stick[2]) < 10){ if((fabs(posiTheta)<7.5)&&(fabs(posiTheta)>1)){ spin(0); pc.printf("a"); }else if(((posiTheta>=7.5)&&(posiTheta<22.5))&&((posiTheta>16)||(posiTheta<14))){ spin(15); pc.printf("b"); }else if(((posiTheta>=22.5)&&(posiTheta<37.5))&&((posiTheta>31)||(posiTheta<29))){ spin(30); }else if(((posiTheta>=37.5)&&(posiTheta<52.5))&&((posiTheta>46)||(posiTheta<44))){ spin(45); }else if(((posiTheta>=52.5)&&(posiTheta<67.5))&&((posiTheta>61)||(posiTheta<59))){ spin(60); }else if(((posiTheta>=67.5)&&(posiTheta<82.5))&&((posiTheta>76)||(posiTheta<74))){ spin(75); }else if(((posiTheta>=82.5)&&(posiTheta<97.5))&&((posiTheta>91)||(posiTheta<89))){ spin(90); }else if(((posiTheta>=97.5)&&(posiTheta<112.5))&&((posiTheta>106)||(posiTheta<104))){ spin(105); }else if(((posiTheta>=112.5)&&(posiTheta<127.5))&&((posiTheta>121)||(posiTheta<119))){ spin(120); }else if(((posiTheta>=127.5)&&(posiTheta<142.5))&&((posiTheta>136)||(posiTheta<134))){ spin(135); }else if(((posiTheta>=142.5)&&(posiTheta<157.5))&&((posiTheta>151)||(posiTheta<149))){ spin(150); }else if(((posiTheta>=157.5)&&(posiTheta<172.5))&&((posiTheta>166)||(posiTheta<164))){ spin(165); }else if(((posiTheta>=172.5)&&(posiTheta<=179))||((posiTheta<=-172.5)&&(posiTheta>=-179))){ spin(180); }else if(((posiTheta<=-157.5)&&(posiTheta>-172.5))&&((posiTheta>-164)||(posiTheta<-166))){ spin(-165); }else if(((posiTheta<=-142.5)&&(posiTheta>-157.5))&&((posiTheta>-149)||(posiTheta<-151))){ spin(-150); }else if(((posiTheta<=-127.5)&&(posiTheta>-142.5))&&((posiTheta>-134)||(posiTheta<-136))){ spin(-135); }else if(((posiTheta<=-112.5)&&(posiTheta>-127.5))&&((posiTheta>-119)||(posiTheta<-121))){ spin(-120); }else if(((posiTheta<=-97.5)&&(posiTheta>-112.5))&&((posiTheta>-104)||(posiTheta<-106))){ spin(-105); }else if(((posiTheta<=-82.5)&&(posiTheta>-90.5))&&((posiTheta>-89)||(posiTheta<-91))){ spin(-90); }else if(((posiTheta<=-67.5)&&(posiTheta>-82.5))&&((posiTheta>-74)||(posiTheta<-76))){ spin(-75); }else if(((posiTheta<=-52.5)&&(posiTheta>-67.5))&&((posiTheta>-59)||(posiTheta<-61))){ spin(-60); }else if(((posiTheta<=-37.5)&&(posiTheta>-52.5))&&((posiTheta>-44)||(posiTheta<-46))){ spin(-45); }else if(((posiTheta<=-22.5)&&(posiTheta>-37.5))&&((posiTheta>-29)||(posiTheta<-30))){ spin(-30); }else if(((posiTheta<=-7.5)&&(posiTheta>-22.5))&&((posiTheta>-14)||(posiTheta<-16))){ spin(-15); }else{ } } /*旋回*/ if(abs(stick[2]) > 10){ spin_power = engine[2]; }else{ spin_power = angle.compute(); } omni.computeXY(engine[0],engine[1],-spin_power); for(int i = 0; i < 4; i++) speed[i] = omni.wheel[i]; for(int i=0; i<4; i++) motor[i].setSpeed(speed[i]); } void spin(double ang) { angle.setSetPoint(ang); stop = 1; posiTheta = posi.getTheta()*(180.0/M_PI); angle.setProcessValue(posiTheta); pc.printf("ang:%.2f fabs:%.2f posi:%.2f\r\n",ang,fabs(ang-posiTheta),posiTheta); //for(int i=4; i<8; i++) motor[i].setSpeed(0);射出機構とかのモーターが出てきたときに[//]を消す }