2022NHKAチーム(射出、紙飛行機折り、昇降)
Dependencies: OmniPosition PID R1370 SerialMultiByte Servo ikarashiMDC_2byte_ver mbed omni_wheel
main.cpp
- Committer:
- nagix
- Date:
- 20 months ago
- Revision:
- 2:f856cbeb5940
- Parent:
- 1:31f47d3fa8cd
File content as of revision 2:f856cbeb5940:
#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); ////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(); int16_t volume[3]; uint8_t toggle[4]; uint8_t timeout; double bldcspeed = 0.6; 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); // // 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}; while(1) { stop = toggle[0]; 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("%3d ", toggle[i]); } // pc.printf(" | "); timeout = data[8]; pc.printf("%3d ", timeout); // pc.printf(" | "); if (mycon.getStatus()) pc.printf("receive"); else{pc.printf("anything error..."); for( int i=0; i<12; i++){ motor[i].setSpeed(0); } } // /*阿部君の素晴らしい天才的な足回り関数*/ // 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]; 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.32; }else if(b[13]){ speed[8] = 0.4; } if(b[11]){ speed[9] = -0.32; }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{ // 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)); //}