NHK2022Aチームの足回りと機構のセット メインプログラム
Dependencies: FEP_RX22 OmniPosition PID R1370 SerialMultiByte Servo ikarashiMDC_2byte_ver omni_wheel
main.cpp
- Committer:
- me33004m
- Date:
- 2022-10-11
- Revision:
- 10:886268a42090
- Parent:
- 9:8dd49bc48d59
- Child:
- 11:10d3dc96c469
File content as of revision 10:886268a42090:
#include "mbed.h" #include "ikarashiMDC.h" #include "pinconfig.h" #include "omni_wheel.h" #include "Servo.h" #include "math.h" #include "SerialMultiByte.h" #include "PID.h" #include "R1370.h" #include "OmniPosition.h" #include "FEP_RX22.h" #include "cmath" #define PI 3.141592653589 #define maxSpeed 0.4 DigitalIn userButton(USER_BUTTON); AnalogIn VOLUME(A0); FEP_RX22 mycon(fepTX, fepRX, fepad); Serial pc(pcTX, pcRX, 115200); Serial RS485(mdcTX,mdcRX,115200); DigitalOut stop(em_stop); SerialMultiByte rcv(sub2TX,sub2RX); ikarashiMDC motor[] = { ikarashiMDC(0,0,SM,&RS485), //asi ikarashiMDC(0,1,SM,&RS485), //asi ikarashiMDC(0,2,SM,&RS485), //asi ikarashiMDC(0,3,SM,&RS485), //asi ikarashiMDC(1,0,SM,&RS485), //全体昇降1 ikarashiMDC(1,1,SM,&RS485), //全体昇降2 ikarashiMDC(1,2,SM,&RS485), //フジモン機構 ikarashiMDC(1,3,SM,&RS485), //フジモン機構 ikarashiMDC(2,0,SM,&RS485), //井上左昇降 ikarashiMDC(2,1,SM,&RS485), //井上右昇降 ikarashiMDC(2,2,SM,&RS485), //井上左前後 ikarashiMDC(2,3,SM,&RS485), //井上右前後 }; Servo pwm_imput1(BLDC1);//ブラシレス宣言 Servo pwm_imput2(BLDC2); Servo pwm_imput3(BLDC3); // angとanglとangleの変数とクラス名がある。気をつけよう*********************重要重要*********************** 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[12]; double engine[4]; double spin_power; double posiX , posiY , posiTheta; int TargetAngle = 0; int sum_1 = 0; int sum_2 = 0; double bldcspeed = 0.8; int16_t angle[4] = {0};//エンコーダ受信格納 uint8_t pulse[8] = {0}; OmniWheel omni(4); OmniPosition posi(sub1TX, sub1RX); PID angl(10.0, 5.0, 0.0000005, 0.01); //プロトタイプ宣言 void chassis(); //機体自体の制御 void spin(double ang); //PID int pm(double num); //正負判定 Timer t; int main(void){ t.start(); rcv.setHeaders(0xff,0xff); rcv.startReceive(4); //SerialMultiByte受信 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); angl.setInputLimits(-180,180); angl.setOutputLimits(-0.4,0.4); angl.setBias(0); angl.setMode(1); angl.setSetPoint(0); while(1){ stop = toggle[0]; chassis(); } } void chassis(){ #if ControllerMode for (int i=0; i<16; i++) b[i] = mycon.getButton(i); for (int i=0; i<4; i++) stick[i] = mycon.getStick(i); for (int i=0; i<2; i++) trigger[i] = mycon.getTrigger(i); // for (int i=0; i<16; i++) pc.printf("%d ", b[i]); // pc.printf(" | "); // for (int i=0; i<4; i++) pc.printf("%3d ", stick[i]); // pc.printf(" | "); // for (int i=0; i<2; i++) pc.printf("%3d ", trigger[i]); // pc.printf(" | "); #else /*ジャイロ読み取り*/ /*足回りのXY座標は多分使わないので無くてもよし。*/ 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; } */ 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); /*初期化*/ 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("%3d ", toggle[i]); } pc.printf(" | "); timeout = data[8]; pc.printf("%3d ", timeout); pc.printf(" | "); #endif if (mycon.getStatus()) pc.printf("receive"); else pc.printf("anything error..."); rcv.getData(pulse); //エンコーダ受信 for(int i=0,j=0;i<4;i++,j+=2){ angle[i] = pulse[j] << 8 | pulse[j+1]; } pc.printf("| ENC1:%d | ENC2:%d | ENC3:%d | ENC4:%d |",angle[0],angle[1],angle[2],angle[3]); if(abs(stick[2])<10){ pc.printf("TA:%.2f pid:%.2f T-p:%.2f",TargetAngle,-angl.compute(),TargetAngle-posiTheta); } pc.printf("\r\n"); //ブラシレスモーター pwm_imput1 = ((double)volume[0]/255.0)*bldcspeed*toggle[1]; pwm_imput2 = ((double)volume[1]/255.0)*bldcspeed*toggle[2]; pwm_imput3 = ((double)volume[2]/255.0)*bldcspeed*toggle[3]; // pc.printf("servo:%.2f | servo2:%.2f | servo3:%.2f\r\n", // ((double)volume[0]/255.0)*bldcspeed,((double)volume[1]/255.0)*bldcspeed,((double)volume[2]/255.0)*bldcspeed); /*井上機構ON,OFF*/ if(toggle[1]){ speed[10] = -0.9; }else{ speed[10] = 0; } if(toggle[1] && b[15]){ speed[10] = 0.4; } if(toggle[3]){ speed[11] = -0.9; }else{ speed[11] = 0; } if(toggle[3] && b[15]){ speed[11] = 0.4; } /*フジモン機構*/ if(toggle[2]){ speed[6] = 0.6; speed[7] = 0.6; }else{ speed[6] = 0; speed[7] = 0; } /*展開昇降*/ if(b[5] != 0){ speed[4] = 0.5; speed[5] = 0.5; }else if(b[4] != 0){ speed[4] = -0.35; speed[5] = -0.35; }else{ speed[4] = 0; speed[5] = 0; } //機構昇降 if(b[9]){ speed[8] = -0.35; }else if(b[13]){ speed[8] = 0.4; } if(b[11]){ speed[9] = -0.35; }else if(b[14]){ speed[9] = 0.4; } if((b[9]!=1) && (b[13]!=1)){ speed[8]=0; } if((b[11]!=1) && (b[14]!=1)){ speed[9]=0; } /*PID*/ if(abs(stick[2]) < 10){ 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[1]/225.0); engine[0] = 0; }else if(b[1]){ engine[0] = -maxSpeed*(trigger[1]/225.0); engine[1] = 0; }else if(b[2]){ engine[1] = -maxSpeed*(trigger[1]/225.0); engine[0] = 0; }else if(b[3]){ engine[0] = maxSpeed*(trigger[1]/255.0); engine[1] = 0; }else{ engine[i] = 0; } } /*旋回*/ if(abs(stick[2]) > 10){ spin_power = engine[2]; }else{ spin_power = angl.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<12; i++) motor[i].setSpeed(speed[i]); } void spin(double ang) { angl.setSetPoint(ang); posiTheta = posi.getTheta()*(180.0/M_PI); angl.setProcessValue(posiTheta); //for(int i=4; i<12; i++) motor[i].setSpeed(0);旋回時以外はPID判定なのでsetSpeed(0)だと機構が動かなくなるので多分このコードいらないです。 //pc.printf("ang:%.2f pid:%.2f posi:%.2f T-p:%.2f\r\n",ang,-angl.compute(),posiTheta,TargetAngle-posiTheta); } int pm(double num){ return((num>0)-(num<0)); }