NHK2022Aチームの足回りと機構のセット メインプログラム
Dependencies: FEP_RX22 OmniPosition PID R1370 SerialMultiByte Servo ikarashiMDC_2byte_ver omni_wheel
main.cpp
- Committer:
- me33004m
- Date:
- 2022-10-09
- Revision:
- 6:d4b82ba4836a
- Parent:
- 5:885bffdceaa2
- Child:
- 7:778eaeae8128
File content as of revision 6:d4b82ba4836a:
#include "mbed.h" #include "ikarashiMDC.h" #include "FEP_RX22.h" #include "pinconfig.h" #include "omni_wheel.h" #include "math.h" #include "OmniPosition.h" #include "PID.h" #define PI 3.141592653589 #define maxSpeed 0.4 FEP_RX22 mycon(fepTX, fepRX, fepad); Serial pc(pcTX, pcRX, 115200); Serial RS485(PC_10,PC_11,115200); uint8_t b[16]; int16_t stick[4]; int16_t trigger[4]; int16_t volume[3]; uint8_t toggle[4]; uint8_t timeout; uint8_t data[128]; int pw; double engine[4]; double speed[6]; double spin_power; double posiX , posiY , posiTheta; int TargetAngle = 0; int StartAngle = 0; DigitalOut stop(PA_5); 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 pm(double num); //正負判定 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 = toggle[0]; chassis(); } } void chassis(){ /*ジャイロ読み取り*/ 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); /*コントローラー受信*/ mycon.getData(data); for (int i=0, tmp=1; i<8; i++) { pw = pow((float)2,i); b[i] = (int)((data[0] & tmp)/pw); pc.printf("%d ", b[i]); tmp *= 2; } for (int i=8, tmp=1, j=0; i<16; i++, j++) { pw = pow((float)2,j); b[i] = (int)((data[1] & tmp)/pw); pc.printf("%d ", b[i]); tmp *= 2; } pc.printf(" | "); for (int i=0; i<4; i++) { stick[i] = data[i+2]; pc.printf("%3d ", stick[i]); } pc.printf(" | "); for (int i=0; i<2; i++) { trigger[i] = data[i+6]; pc.printf("%3d ", trigger[i]); } pc.printf(" | "); for (int i=0; i<3; i++) { volume[i] = data[i+9]; pc.printf("%3d ", volume[i]); } pc.printf(" | "); for (int i=0; i<4; i++) { toggle[i] = data[i+12]; pc.printf("%d ", toggle[i]); } pc.printf(" | "); timeout = data[8]; pc.printf("%3d ", timeout); pc.printf(" | "); if(mycon.getStatus()) 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++){ stick[i] = stick[i] - 128; } /*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{ }*/ /*for(int i=-180;i<=180;i+=15){ if(((posiTheta>=i-7.5)&&(posiTheta<i+7.5))&&((posiTheta>i+1)||(posiTheta<i-1))){ spin(i); } else if((posiTheta == i) ||(posiTheta == i+1) ||(posiTheta == i-1)){ } }*/ // if(fabs(posiTheta)<8){ // spin(StartAngle); // } // if(fabs(TargetAngle-posiTheta)<8){ // spin(TargetAngle); // } if(fabs(TargetAngle-posiTheta)>8){ TargetAngle += 15*pm(posiTheta-TargetAngle); if(abs(TargetAngle)==195){//abs(TargetAngle)==180だと永遠にこのif文に捕らわれてしまいそう。 TargetAngle += -360*pm(TargetAngle); } } spin(TargetAngle); } /*全方位*/ for(int i=0; i<4; i++){ if(abs(stick[i]) > 10){ if(trigger[1]<10) trigger[1] = 0; engine[i] = maxSpeed*(stick[i]/128.0)*(trigger[1]/255.0); }else if(b[i]%2){ if(trigger[1]<10) trigger[1] = 0; //b[1]のとき左向き(負)b[3]のとき右向き(正) engine[0] = maxSpeed*pm(i-2)*(trigger[1]/255.0); engine[1] = 0; }else if(b[i]%2==0){ if(trigger[1]<10) trigger[1] = 0; //b[0]のとき上向き(正)b[2]のとき下向き(負) engine[1] = -maxSpeed*pm(i-1)*(trigger[1]/255.0); engine[0] = 0; }else{ engine[i] = 0; } } /*旋回*/ 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); posiTheta = posi.getTheta()*(180.0/M_PI); angle.setProcessValue(posiTheta); pc.printf("ang:%.2f pid:%.2f posi:%.2f T-p:%.2f\r\n",ang,-angle.compute(),posiTheta,TargetAngle-posiTheta); //for(int i=4; i<12; i++) motor[i].setSpeed(0);射出機構とかのモーターが出てきたときに[//]を消す } int pm(double num){ return((num>0)-(num<0)); }