![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
fsaf
Fork of MPU6050_Driver_Balanceddddd by
main.cpp
- Committer:
- glintligo
- Date:
- 2018-04-20
- Revision:
- 1:f9658c7309ef
- Parent:
- 0:badebd32bd8b
- Child:
- 2:3a7eb05dbc72
File content as of revision 1:f9658c7309ef:
#include "mbed.h" #include "mpu6050.h" #define PWM_PERIOD 1 #define RATE 0.01 float NTMAX=100.0; struct PID { float Kp; float Ki; float Kd; float error; float dif_error; float last_error; float int_error; }; //0.00005 //0.00015 PID PID_Para={0.0,0.0,0.0005}; Ticker toggle_led_ticker; //声明一个 Ticker 对象 DigitalOut myled(PC_13); //Serial pc(PA_9, PA_10); float pitch,roll,yaw,co; //欧拉角 PwmOut mypwm1(PA_7); PwmOut mypwm2(PA_6); PwmOut mypwm3(PB_0); PwmOut mypwm4(PB_1); float calculate(float input,float target); void make() { //中断子程序,在平衡车和无人机的项目中,控制算法就写在中断子程序中:获取陀螺仪数据,根据陀螺仪数据通过PID算法得到电机的控制量(占空比和方向),在输出到单片机管脚 co = calculate(roll, 0.0); // pc.printf("pitch: %.2f roll: %.2f yaw: %.2f co: %.2f\r\n", pitch,roll,yaw,co); // pc.printf("roll: %.2f co: %.2f\r\n",roll,co); if (co >=0) { mypwm2.pulsewidth(co); mypwm1.pulsewidth(0); mypwm4.pulsewidth(co); mypwm3.pulsewidth(0);} else { mypwm2.pulsewidth(0); mypwm1.pulsewidth(-co); mypwm4.pulsewidth(0); mypwm3.pulsewidth(-co);} } float calculate(float input,float target) { PID_Para.error=target-input; PID_Para.dif_error=PID_Para.error-PID_Para.last_error; PID_Para.last_error=PID_Para.error; PID_Para.int_error+=PID_Para.error; if(PID_Para.int_error >= NTMAX) PID_Para.int_error = NTMAX; if(PID_Para.int_error <= -NTMAX) PID_Para.int_error = -NTMAX; float output; output=PID_Para.Kp*PID_Para.error+PID_Para.Ki*PID_Para.int_error-PID_Para.Kd*PID_Para.dif_error; return output; } int main() { mypwm1.period_ms(PWM_PERIOD); mypwm2.period_ms(PWM_PERIOD); mypwm3.period_ms(PWM_PERIOD); mypwm4.period_ms(PWM_PERIOD); MPU_Init(); //初始化MPU6050 myled = 0; while(mpu_dmp_init()) { wait(0.2); myled = !myled; } toggle_led_ticker.attach(&make, RATE); while(1) { mpu_dmp_get_data(&pitch,&roll,&yaw); } }