liang brain / Mbed 2 deprecated Ex_Balance_Carcode_hukeqin

fdsaf

Dependencies:   mbed

main.cpp@1:db5ac8bf9280, 2018-05-20 (annotated)

Committer:
glintligo
Date:
Sun May 20 07:02:38 2018 +0000
Revision:
1:db5ac8bf9280
Parent:
0:badebd32bd8b 
????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
heroistired 0:badebd32bd8b 1 #include "mbed.h"
heroistired 0:badebd32bd8b 2 #include "mpu6050.h"
glintligo 1:db5ac8bf9280 3 #define PWM_PERIOD 1 //pwm波的周期 单位ms
glintligo 1:db5ac8bf9280 4 #define RATE 0.01 //定时中断的周期，单位s
glintligo 1:db5ac8bf9280 5
glintligo 1:db5ac8bf9280 6
glintligo 1:db5ac8bf9280 7 struct PID
glintligo 1:db5ac8bf9280 8 {
glintligo 1:db5ac8bf9280 9 float Kp;
glintligo 1:db5ac8bf9280 10 float Ki;
glintligo 1:db5ac8bf9280 11 float Kd;
glintligo 1:db5ac8bf9280 12 float error;
glintligo 1:db5ac8bf9280 13 float dif_error;
glintligo 1:db5ac8bf9280 14 float last_error;
glintligo 1:db5ac8bf9280 15 float int_error;
glintligo 1:db5ac8bf9280 16 };
glintligo 1:db5ac8bf9280 17
glintligo 1:db5ac8bf9280 18
glintligo 1:db5ac8bf9280 19 float NTMAX=100.0; //最大容错
glintligo 1:db5ac8bf9280 20 float t =0.0; //pid控制的目标
glintligo 1:db5ac8bf9280 21 PID PID_Para={0.00008,0.0,-0.00057}; //pid参数,kp,kp,kd
glintligo 1:db5ac8bf9280 22 float pitch,roll,yaw; //欧拉角
glintligo 1:db5ac8bf9280 23 float co; //pid输出结果，作为pwm波的输入
heroistired 0:badebd32bd8b 24
heroistired 0:badebd32bd8b 25
glintligo 1:db5ac8bf9280 26 Ticker toggle_time_ticker; //时间中断，用来控制转速的变化
glintligo 1:db5ac8bf9280 27 DigitalOut myled(PC_13); //检测mpu是否正常初始化，正常初始化后不会闪烁
glintligo 1:db5ac8bf9280 28 Serial pc(PA_9, PA_10); //蓝牙串口
glintligo 1:db5ac8bf9280 29 PwmOut mypwm1(PA_7); //控制电机的pwm波
glintligo 1:db5ac8bf9280 30 PwmOut mypwm2(PA_6); //控制电机的pwm波
glintligo 1:db5ac8bf9280 31 PwmOut mypwm3(PB_0); //控制电机的pwm波
glintligo 1:db5ac8bf9280 32 PwmOut mypwm4(PB_1); //控制电机的pwm波
glintligo 1:db5ac8bf9280 33
glintligo 1:db5ac8bf9280 34 /**
glintligo 1:db5ac8bf9280 35 * [calculate 计算pid控制结果]
glintligo 1:db5ac8bf9280 36 * @param input [pid的输入]
glintligo 1:db5ac8bf9280 37 * @param target [pid控制目标]
glintligo 1:db5ac8bf9280 38 * @return [pid的输出]
glintligo 1:db5ac8bf9280 39 */
glintligo 1:db5ac8bf9280 40 float calculate(float input,float target);
glintligo 1:db5ac8bf9280 41 /**
glintligo 1:db5ac8bf9280 42 * [make 时间中断函数]
glintligo 1:db5ac8bf9280 43 */
glintligo 1:db5ac8bf9280 44 void make();
glintligo 1:db5ac8bf9280 45
heroistired 0:badebd32bd8b 46
heroistired 0:badebd32bd8b 47 int main() {
glintligo 1:db5ac8bf9280 48 //设定pwm波的周期 为1ms
glintligo 1:db5ac8bf9280 49 mypwm1.period_ms(PWM_PERIOD);
glintligo 1:db5ac8bf9280 50 mypwm2.period_ms(PWM_PERIOD);
glintligo 1:db5ac8bf9280 51 mypwm3.period_ms(PWM_PERIOD);
glintligo 1:db5ac8bf9280 52 mypwm4.period_ms(PWM_PERIOD);
heroistired 0:badebd32bd8b 53
glintligo 1:db5ac8bf9280 54 //初始化MPU6050，正常初始化后不会闪烁
glintligo 1:db5ac8bf9280 55 MPU_Init();
heroistired 0:badebd32bd8b 56 myled = 0;
heroistired 0:badebd32bd8b 57 while(mpu_dmp_init())
heroistired 0:badebd32bd8b 58 {
heroistired 0:badebd32bd8b 59 wait(0.2);
heroistired 0:badebd32bd8b 60 myled = !myled;
heroistired 0:badebd32bd8b 61 }
glintligo 1:db5ac8bf9280 62
glintligo 1:db5ac8bf9280 63 //设置定时中断的周期为100hz，调用中断函数为void make()
glintligo 1:db5ac8bf9280 64 toggle_time_ticker.attach(&make, RATE);
glintligo 1:db5ac8bf9280 65
heroistired 0:badebd32bd8b 66 while(1)
heroistired 0:badebd32bd8b 67 {
glintligo 1:db5ac8bf9280 68 //获取mpu数据，roll为俯仰角
glintligo 1:db5ac8bf9280 69 mpu_dmp_get_data(&pitch,&roll,&yaw);
glintligo 1:db5ac8bf9280 70 }
glintligo 1:db5ac8bf9280 71
glintligo 1:db5ac8bf9280 72 }
glintligo 1:db5ac8bf9280 73
glintligo 1:db5ac8bf9280 74 /**
glintligo 1:db5ac8bf9280 75 * [make 时间中断函数]
glintligo 1:db5ac8bf9280 76 */
glintligo 1:db5ac8bf9280 77 void make() {
glintligo 1:db5ac8bf9280 78 //计算pid控制结果
glintligo 1:db5ac8bf9280 79 co = calculate(roll, t);
glintligo 1:db5ac8bf9280 80 //控制电机
glintligo 1:db5ac8bf9280 81 if (co >=0)
glintligo 1:db5ac8bf9280 82 {
glintligo 1:db5ac8bf9280 83 mypwm1.pulsewidth(co);
glintligo 1:db5ac8bf9280 84 mypwm2.pulsewidth(0);
glintligo 1:db5ac8bf9280 85 mypwm3.pulsewidth(co);
glintligo 1:db5ac8bf9280 86 mypwm4.pulsewidth(0);
glintligo 1:db5ac8bf9280 87 }
glintligo 1:db5ac8bf9280 88 else
glintligo 1:db5ac8bf9280 89 {
glintligo 1:db5ac8bf9280 90 mypwm1.pulsewidth(0);
glintligo 1:db5ac8bf9280 91 mypwm2.pulsewidth(-co);
glintligo 1:db5ac8bf9280 92 mypwm3.pulsewidth(0);
glintligo 1:db5ac8bf9280 93 mypwm4.pulsewidth(-co);
glintligo 1:db5ac8bf9280 94 }
heroistired 0:badebd32bd8b 95 }
glintligo 1:db5ac8bf9280 96
glintligo 1:db5ac8bf9280 97 /**
glintligo 1:db5ac8bf9280 98 * [calculate 计算pid控制结果]
glintligo 1:db5ac8bf9280 99 * @param input [pid的输入]
glintligo 1:db5ac8bf9280 100 * @param target [pid控制目标]
glintligo 1:db5ac8bf9280 101 * @return [pid的输出]
glintligo 1:db5ac8bf9280 102 */
glintligo 1:db5ac8bf9280 103 float calculate(float input,float target)
glintligo 1:db5ac8bf9280 104 {
glintligo 1:db5ac8bf9280 105 //计算输入与目标的偏差
glintligo 1:db5ac8bf9280 106 PID_Para.error=target-input;
glintligo 1:db5ac8bf9280 107 //设置死位
glintligo 1:db5ac8bf9280 108 if(PID_Para.error<1.0 && PID_Para.error >-1.0) PID_Para.error=0.0;
glintligo 1:db5ac8bf9280 109 //计算微分
glintligo 1:db5ac8bf9280 110 PID_Para.dif_error=PID_Para.error-PID_Para.last_error;
glintligo 1:db5ac8bf9280 111 //后一位偏差
glintligo 1:db5ac8bf9280 112 PID_Para.last_error=PID_Para.error;
glintligo 1:db5ac8bf9280 113 //计算积分
glintligo 1:db5ac8bf9280 114 PID_Para.int_error+=PID_Para.error;
glintligo 1:db5ac8bf9280 115 //限制积分范围
glintligo 1:db5ac8bf9280 116 if(PID_Para.int_error >= NTMAX)
glintligo 1:db5ac8bf9280 117 PID_Para.int_error = NTMAX;
glintligo 1:db5ac8bf9280 118 if(PID_Para.int_error <= -NTMAX)
glintligo 1:db5ac8bf9280 119 PID_Para.int_error = -NTMAX;
glintligo 1:db5ac8bf9280 120
glintligo 1:db5ac8bf9280 121 float output;
glintligo 1:db5ac8bf9280 122 //计算pid结果
glintligo 1:db5ac8bf9280 123 output=PID_Para.Kp*PID_Para.error+PID_Para.Ki*PID_Para.int_error-PID_Para.Kd*PID_Para.dif_error;
glintligo 1:db5ac8bf9280 124 return output;
glintligo 1:db5ac8bf9280 125 }