![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
2022NHKAチーム(射出、紙飛行機折り、昇降)
Dependencies: OmniPosition PID R1370 SerialMultiByte Servo ikarashiMDC_2byte_ver mbed omni_wheel
Diff: main.cpp
- Revision:
- 1:31f47d3fa8cd
- Parent:
- 0:b0ca7b23bdb5
- Child:
- 2:f856cbeb5940
diff -r b0ca7b23bdb5 -r 31f47d3fa8cd main.cpp --- a/main.cpp Sun Oct 09 08:59:22 2022 +0000 +++ b/main.cpp Mon Oct 10 05:43:42 2022 +0000 @@ -12,6 +12,7 @@ #include "cmath" #define PI 3.141592653589 +#define maxSpeed 0.4 DigitalIn userButton(USER_BUTTON); AnalogIn VOLUME(A0); @@ -33,46 +34,72 @@ 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), + 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); -OmniWheel omni(4);//足回り宣言 +//abe 足回り +double engine[4]; +double spin_power; +double posiX , posiY , posiTheta; +int TargetAngle = 0; +int StartAngle = 0; + +OmniWheel omni(4); OmniPosition posi(sub1TX, sub1RX); +PID angle(10.0, 5.0, 0.0000005, 0.01); + +//プロトタイプ宣言 +void chassis(); //足回りの制御・ジャイロ・テラターム +void spin(double ang); //PID +int pm(double num); //正負判定 + Timer t; +int16_t trigger[4]; +uint8_t b[16]; +int16_t stick[4]; +uint8_t data[128]; +int pw; + +double speed[12] = {0}; + int main() { t.start(); - - double bldcspeed = 0.8; + + int16_t volume[3]; + uint8_t toggle[4]; + uint8_t timeout; -// double spin_power; //足回り宣言 -// double posiX , posiY , posiTheta; + double bldcspeed = 0.6; rcv.setHeaders(0xff,0xff); rcv.startReceive(4); //SerialMultiByte受信 mycon.StartReceive(); //コントローラー受信・宣言 - 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] = {0}; + //足回り宣言 + 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); + int16_t angle[4] = {0};//エンコーダ受信格納 uint8_t pulse[8] = {0}; @@ -81,73 +108,60 @@ { stop = toggle[0]; - 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]); - pc.printf("\r\n"); - -#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 - mycon.getData(data); + 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]); +// 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]); +// pc.printf("%d ", b[i]); tmp *= 2; } - pc.printf(" | "); - +// pc.printf(" | "); for (int i=0; i<4; i++) { stick[i] = data[i+2]; - pc.printf("%3d ", stick[i]); +// pc.printf("%3d ", stick[i]); } - pc.printf(" | "); - +// pc.printf(" | "); for (int i=0; i<2; i++) { trigger[i] = data[i+6]; - pc.printf("%3d ", trigger[i]); +// pc.printf("%3d ", trigger[i]); } - pc.printf(" | "); - +// 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("%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(" | "); +// pc.printf(" | "); + if (mycon.getStatus()) pc.printf("receive"); + else{pc.printf("anything error..."); + for( int i=0; i<12; i++){ + motor[i].setSpeed(0); + } + } -#endif - if (mycon.getStatus()) pc.printf("receive"); - else pc.printf("anything error..."); - + /*阿部君の素晴らしい天才的な足回り関数*/ + chassis(); + + 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]); + pc.printf("\r\n"); //ブラシレスモーター pwm_imput1 = ((double)volume[0]/255.0)*bldcspeed*toggle[1]; @@ -158,34 +172,136 @@ // 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.3; - speed[5] = 0.3; + speed[4] = 0.5; + speed[5] = 0.5; }else if(b[4] != 0){ - speed[4] = -0.3; - speed[5] = -0.3; + speed[4] = -0.35; + speed[5] = -0.35; }else{ speed[4] = 0; speed[5] = 0; } //機構昇降 - if(b[9] != 0){ - speed[6] = 0.3; - }else if(b[11] != 0){ - speed[7] = 0.3; - }else if(b[13] != 0){ - speed[6] = -0.3; - }else if(b[14] != 0){ - speed[7] = -0.3; + 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; + } + for(int i=0; i<12; i++) pc.printf("%.2f ",speed[i]); + for(int i=4; i<12; i++) motor[i].setSpeed(speed[i]); + } + +} + +void chassis(){ + + mycon.getData(data); + for (int i=0; i<4; i++) { + stick[i] = data[i+2]; + } + + /*ジャイロ読み取り*/ + 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); + + 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){ + 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{ - speed[6] = 0; - speed[7] = 0; + spin_power = angle.compute(); } - for(int i=0; i<6; i++) motor[i].setSpeed(speed[i]); - + 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]); - } - -} \ No newline at end of file +} + + +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)); +} \ No newline at end of file