不想错过我的推送，右上角-查看公众号-设为星标
整个模块分上下三层：最下层为亚克力板固定四个8520电机，中间层为电机驱动电路板、各个模块接口等，上层为主控核心板；
电机两个正转，两个反转，分别对应X、Y的+/-两个方向；螺旋桨两正两反，在这里螺旋桨做推力方向，可以良好的避免异常情况下的射桨等极端情况；
传感器：MPU6050陀螺仪模块
点状激光头 –固定在底部亚克力板上
电机使用8520空心杯电机，电气参数如下：
电机型号：8520空心杯电机（2S） 重量：5.3g
工作电压：5~7.4V DC 堵转电流：8A
空载电流：100mA 空载转速：50000RPM
负载电流：1.2A 负载转速：44000RPM
当只有一个方向的水平摆动的情形如x方向的摆动时；根据定义，我们可将系统理想化为单摆模型，因此有：
上述（1）为简谐运动的典型运动学公式，质点做简谐运动，其位移与时间的函数关系；
f(x)：表示位移
A：表示振幅
w：表示角速度
φ：表示相位
周期T与w之间的关系为：T=2π/w。
为单摆周期计算公式，结合实物参数推算大致的运动周期：
L=摆杆长度=0.3m
g=重力加速度，我们取10m/s2
因此，推导出下面公式：
显然，上面的摆杆我们选取的长度是相对比较短的，假设极端一点，我们选取摆杆长度为2m，此时公式：
周期T的取值范围：1.09s ~2.81s根据单摆周期的实验法去测得周期：这里我们测量的是十次单摆周期秒表计时为11.5s；因此在程序中定义
#define Period 1150 //单摆周期 ms
如果修改了摆杆长度、机械结构，这个值要去重新测得，然后看公式（1）中的变量
A：振幅，是我们需要设定的摆动幅度；
φ：相位，在这里可以理解为开始时对应的位移，我们设为0；
w：角速度，w=2π/T 已知
t：自变量，u32 TimeCnt;//运动时间计数器
综上，可以推导出f(x)
离地高度 float Height=675（单位 mm）；
角度由陀螺仪模块反馈获得，X 方向为 Roll，Y 方向为 Pitch
激光点位置 float Measure_X，可以通过测得的角度三角转换得出测量值：Measure_X=tan(Roll)Height；加入初始值、弧度单位换算即：Measure_X=(float)tan((Roll-ZHONGZHI_B)/1802PI)Height;
控制激光点的位置满足单摆运动因此有目标值 Target_X：
Alpha=(float)TimeCnt/Period2PI;
Target_X=Amplitude_xsin(Alpha); //X 方向目标值函数 PID 公式 ：pwm=Kpe(k)+Ki*∑e(k)+Kd[e（k）-e(k-1)]
此时可以通过控制 Amplitude_x 来控制摆动的幅度，导入 PID 这里就不做具体的叙述了；这样就可以控制风力摆每个方向做单摆运动（简谐运动）；
定义：李萨如图形由在互相垂直的方向上的两个频率成简单整数比的简谐振动所合成的规则的、稳定的闭合曲线；
要点：互相垂直、 简谐运动、频率成整数比
系统满足要求，我们来看下李萨如图形的数学定义：
若a=b，n=1，则曲线是椭圆
若 φ=π/2 ，则这椭圆其实是圆
若 φ=0 ，则这椭圆其实是线段
对应我们的需求，回到我们风力摆系统中，当风力摆同时参与两个互相垂直方向上的简谐运动，风力摆的位移是这两个振动的矢量和：
n=1，我们没有必要去调节频率比；因此有我们的目标值函数：Target_X=Amplitude_xsin(Alpha); Target_Y=Amplitude_ysin(Alpha+Phase);
画直线，相位差φ=0；
画圆，a=b，n=1，φ=π/2或者3π/2
这部分内容较多，这里就不展开讲解了，详细的可以查看该资料：https://mbd.pub/o/bread/ZJiakp5y
int Position_PID_X (float value,float Target)
{
static float Bias,Pwm,Integral_bias,Last_Bias;
Bias=value-Target; //计算偏差
Integral_bias+=Bias; //求出偏差的积分
Pwm=Position_KP*Bias+ //PID控制器比例项
Position_KI*Integral_bias+ //PID控制器积分项
Position_KD*(Bias-Last_Bias); //PID控制器微分项
Last_Bias=Bias; //保存上一次偏差
return Pwm; //增量输出
}
int Position_PID_Y (float value,float Target)
{
static float Bias,Pwm,Integral_bias,Last_Bias;
Bias=value-Target; //计算偏差
Integral_bias+=Bias; //求出偏差的积分
Pwm=Position_Kp*Bias+ //PID控制器比例项
Position_Ki*Integral_bias+ //PID控制器积分项
Position_Kd*(Bias-Last_Bias); //PID控制器微分项
Last_Bias=Bias; //保存上一次偏差
return Pwm; //增量输出
}
void Read_DMP(void)
{
unsigned long sensor_timestamp;
unsigned char more;
long quat[4];
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
if (sensors & INV_WXYZ_QUAT )
{
q0=quat[0] / q30;
q1=quat[1] / q30;
q2=quat[2] / q30;
q3=quat[3] / q30;
Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;//yaw
}
}
https://mbd.pub/o/bread/ZJiakp5y
