q
Dependencies: FEP_RX22 OmniPosition PID R1370 ikarashiMDC_2byte_ver omni_wheel
main.cpp
- Committer:
- me33004m
- Date:
- 23 months ago
- Revision:
- 0:b7dbdc0b19f3
File content as of revision 0:b7dbdc0b19f3:
#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 speed[6]; double engine[4]; double spin_power; double posiX , posiY , posiTheta; int TargetAngle = 0; int StartAngle = 0; int sum_1 = 0; int sum_2 = 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]); 上のポインタの式が分からんので無理やり10進数に変える sum_1 += b[i]*pow((float)2,i+1); tmp *= 2; } pc.printf("%3d ",sum_1); /*初期化*/ sum_1 = 0; 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]); sum_2 += b[i]*pow((float)2,i-7); tmp *= 2; } pc.printf("%3d ",sum_2); pc.printf(" | "); /*初期化*/ sum_2 = 0; pc.printf(" | "); for (int i=0; i<4; i++) { stick[i] = (data[i+2] - 128)*(-1); 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); } } /*PID*/ if(abs(stick[2]) < 10){ /*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(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){ engine[i] = maxSpeed*(stick[i]/128.0); }else if(b[0]){ engine[1] = maxSpeed*(trigger[0]/225.0); engine[0] = 0; }else if(b[1]){ engine[0] = -maxSpeed*(trigger[0]/225.0); engine[1] = 0; }else if(b[2]){ engine[1] = -maxSpeed*(trigger[0]/225.0); engine[0] = 0; }else if(b[3]){ engine[0] = maxSpeed*(trigger[0]/255.0); engine[1] = 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); //for(int i=4; i<12; i++) motor[i].setSpeed(0);射出機構とかのモーターが出てきたときに[//]を消す pc.printf("ang:%.2f pid:%.2f posi:%.2f T-p:%.2f\r\n",ang,-angle.compute(),posiTheta,TargetAngle-posiTheta); } int pm(double num){ return((num>0)-(num<0)); }