2022NHKAチーム(射出、紙飛行機折り、昇降)
Dependencies: OmniPosition PID R1370 SerialMultiByte Servo ikarashiMDC_2byte_ver mbed omni_wheel
main.cpp
- Committer:
- nagix
- Date:
- 23 months ago
- Revision:
- 0:b0ca7b23bdb5
- Child:
- 1:31f47d3fa8cd
File content as of revision 0:b0ca7b23bdb5:
#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 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); OmniWheel omni(4);//足回り宣言 OmniPosition posi(sub1TX, sub1RX); Timer t; int main() { t.start(); double bldcspeed = 0.8; // double spin_power; //足回り宣言 // double posiX , posiY , posiTheta; 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}; int16_t angle[4] = {0};//エンコーダ受信格納 uint8_t pulse[8] = {0}; while(1) { 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); 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(" | "); #endif if (mycon.getStatus()) pc.printf("receive"); else pc.printf("anything error..."); //ブラシレスモーター 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); /*展開昇降*/ if(b[5] != 0){ speed[4] = 0.3; speed[5] = 0.3; }else if(b[4] != 0){ speed[4] = -0.3; speed[5] = -0.3; }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; }else{ speed[6] = 0; speed[7] = 0; } for(int i=0; i<6; i++) motor[i].setSpeed(speed[i]); } }