![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
2021NHK main program move and shoot
Dependencies: 2021Bcon omni_wheel prototype01 PID ikarashiMDC_2byte_ver OmniPosition S-ShapeModelFloat Servo_softpwm NHK2021_ikarashiSV
main.cpp
- Committer:
- piroro4560
- Date:
- 2021-10-24
- Revision:
- 0:2b93017d0053
- Child:
- 1:a6aa0b47c9ae
File content as of revision 0:2b93017d0053:
#include "mbed.h" #include "ikarashiMDC.h" #include "ikarashiSV.h" #include "controller.h" #include "pinconfig.h" #include "omni_wheel.h" #include "Servo.h" #include "math.h" #include "position_controller.h" #include "OmniPosition.h" #include "PID.h" #define PI 3.141592653589 Bcon mycon(fepTX, fepRX, fepad); Serial pc(pcTX, pcRX, 115200); DigitalOut stop(em_stop); ikarashiSV slvT(slv1,slv2, slv3,slv4); ikarashiSV2 slvU(slv5,slv6); Servo servo(servo1); //DigitalOut test3(PA_5); //DigitalOut test4(PB_10); //DigitalOut test1(PB_5); //DigitalOut test2(PB_4); //PID angle(2.0,0.0,0.00001,0.01); PID angle(10.0, 0.0, 0.001, 0.01); OmniWheel omni(4); OmniPosition posi(mwTX, mwRX); Serial RS485(mdcTX,mdcRX,115200); ikarashiMDC motor[] = { ikarashiMDC(0,0,SM,&RS485),/*足*/ ikarashiMDC(0,1,SM,&RS485),/*足*/ ikarashiMDC(0,2,SM,&RS485),/*足*/ ikarashiMDC(0,3,SM,&RS485),/*足*/ ikarashiMDC(1,0,SM,&RS485),/*腕昇降*/ ikarashiMDC(1,1,SM,&RS485),/*腕前後*/ }; PositionController pcon[] = { PositionController(200.0, 200.0, 0.2, 0.0, 0.6), PositionController(500.0, 500.0, 0.1, 0.0, 0.20), PositionController(1.0, 13.0, 0.14, 0.05, 0.15), PositionController(100.0, 100.0, 0.11, 0.0, 0.20), PositionController(300, 400, 0.1, 0.0, 0.2), PositionController(700, 1000, 0.1, 0.0, 0.25), PositionController(1500.0, 1500.0, 0.1, 0.0, 0.30), PositionController(200, 200, 0.1, 0.0, 0.20) }; void add(){ // slv.add_state(); } int main(void){ mycon.StartReceive(); uint8_t b[8]; int16_t stick[4]; double accele[4]; double speedX, speedY, speed[6]; double spin_power; double X, Y, R; int val; long int xycnt=0, svcnt=0; servo.calibrate(0.00095, 90); servo.write(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(-3.14,3.14); angle.setOutputLimits(-0.6,0.6); angle.setBias(0); angle.setMode(1); angle.setSetPoint(0); while (1) { for(int i=0; i<6; i++){ speed[i] = 0; } // 非常停止 stop = 1; // コントローラー受信 for (int i=0; i<8; i++) b[i] = mycon.getButton(i); for (int i=0; i<4; i++) stick[i] = mycon.getStick(i); // 腕昇降 if(b[0] != 0){ speed[4] = 0.2; }else if(b[2] != 0){ speed[4] = -0.2; }else{ speed[4] = 0; } // 腕前後 if(b[3] != 0){ speed[5] = 0.2; }else if(b[1] != 0){ speed[5] = -0.2; }else{ speed[5] = 0; } // 計測輪 X = posi.getX(); Y = posi.getY(); R = posi.getTheta(); // PID angle.setProcessValue(R); spin_power = -angle.compute(); // s字制御 if (xycnt==300) pcon[0].targetXY(0, -1000); pcon[0].compute(X, Y); spin_power = -angle.compute(); omni.computeXY(pcon[0].getVelocityX(), pcon[0].getVelocityY(), spin_power); for (int i = 0; i < 4; i++) speed[i] = omni.wheel[i]; // 非常停止テスト用 // for (int i=0; i<4; i++) speed[i] = spin_power; // for (int i=0; i<4; i++) speed[i] = 0; // モーター出力 for(int i=0; i<6; i++) motor[i].setSpeed(speed[i]); // 以下ソレノイド if ((X > -50 && X < 50) && (Y > -1050 && Y < -950)) svcnt++; // svcnt++; if (svcnt == 300) slvU.solenoid(1); if (svcnt == 500) slvT.solenoid(1); if (svcnt == 600) slvT.solenoid(2); if (svcnt == 800) slvT.solenoid(0); if (svcnt == 1000) slvU.solenoid(0); // イカサーボ if(b[7] != 0){ servo.write(0.3); }else{ servo.write(0); } // 表示部 // コントローラ /* if (mycon.status) { pc.printf("received"); // for (int i=0; i<8; i++) pc.printf("%d ", b[i]); pc.printf("%d ", mycon.sum); pc.printf(" | "); for (int i=0; i<4; i++) pc.printf("%3d ", stick[i]); pc.printf(" | "); } else pc.printf("anything error... | "); */ // 計測輪 pc.printf("x: %5d | y: %5d | r: %3.2f | Vx: %f | Vy: %f " , posi.getX(), posi.getY(), posi.getTheta(), pcon[0].getVelocityX(), pcon[0].getVelocityY()); // スピード pc.printf("spd: "); for (int i=0; i<6; i++) pc.printf("%d: %2.2f | ", i, speed[i]); // pc.printf("xycnt: %d | svcnt: %d", xycnt, svcnt); pc.printf("\r\n"); xycnt++; } }