风力摆？这是不是太简单了点
关于风力摆
风力摆是2015年的电子设计大赛题目
关于风力摆的说明啊，实现的功能啊我就不多说了
网上有很多的资料，说难了这是PID调节什么的，但是说简单了这就是一个单摆 和 圆锥摆模型
如果你用PDI一点一点调，你会调死，要是用我的方法，呵呵，往下看吧
废话我就不多说了直接进入正题
所需材料
材料就简单了
- 单片机最小系统（最好用stm32 快，开源，资料多）
- 电机驱动模块2个双通道的（淘宝上很多 几块钱）
- 液晶显示屏（OLED足够用了）
- JLink ( 下载程序用的，为了节约成本用串口下载也可以 )
- 碳杆60cm
- 万向节
- 陀螺仪（mpu6050）
- 空心杯电机（4个）
- 导线若干
- 还有就是电池
先来个图片吧
虽然有点抽，但完全不影响功能
风力摆思路
1. 画直线就是单摆思路
陀螺仪可以测得角加速度，同时经过四元数换算可以得到三轴方向的角度，有了这个基础就简单了。
1.1首先先弄出0°方向的直线
我选的是roll作为0°轴，这样可以先用此时角度算出此时应该要的角速度，然后写两个环，角度负反馈环 和 角速度负反馈环，就可以平衡了，
1.2直线长度可设置
直线长度就和顶点到地面距离L，期望摆起的角度有关
1.3各个方向
然后通过希望的方向角度，可以算出，x, y （x,y 可以根据自己的喜好设置） 方向上所期望的角度和角速度（这个角度满足公式 tan（a）= y / x ， 不清楚的慢慢推，中学物理），就可以向180度延申。
1.4停止就只要吧期望线长设为0
2.画圆的思路
和画直线是一样的，由圆锥摆的性质，可得，摆长一定，摆起的高度和角速度成比例关系，
要注意的是这里的角速度和mpu6050的角速度不等价，这里mpu6050测出的角速度是旋转时的线速度。这个地方不懂的可以多想想。（我可是吃了亏的，白白延迟了半天的进度）
这里也可以向上面那样加两个负反馈环，不过我就用了一个角速度环，简单嘛。
然后就没有然后，这样就搞定了，附上控制代码吧
1 #include "mymath.h" 2 #include "math.h" 3 #include "IMU6050.h" 4 #include "control.h" 5 6 //================================================单摆 7 //得到此时的偏差值 8 float Get_Offset(Imu_t* imu_t, ALL_Expect* expect, ALL_OFFSET* offset) 9 { 10 float expect_x, expect_y; 11 float speed; 12 float speed_x, speed_y; 13 int dir_x = 0, dir_y = 0; 14 15 dir_x = Get_Direct(expect->angle, imu_t->gyro.x, imu_t->roll, imu_t->pitch, 0); 16 dir_y = Get_Direct(expect->angle, imu_t->gyro.y, imu_t->roll, imu_t->pitch, 1); 17 Get_Expect_X_Y(expect->angle, expect->length_to_angle, &expect_x, &expect_y); 18 19 speed = Get_Speed(expect->length_to_angle, imu_t->roll, imu_t->pitch, 60); 20 Get_Expect_X_Y(expect->angle, speed, &speed_x, &speed_y); 21 22 if(speed>0) 23 { 24 if((imu_t->gyro.x)*(imu_t->roll)>0) 25 { 26 offset->x_offset = MY_ABS(expect_x - MY_ABS(imu_t->roll))*dir_x*1.0; 27 offset->y_offset = MY_ABS(expect_y - MY_ABS(imu_t->pitch))*dir_y*1.0; 28 } 29 else 30 { 31 offset->x_offset = 0; 32 offset->y_offset = 0; 33 } 34 } 35 else if(speed < 0) 36 { 37 38 offset->x_offset = -MY_ABS(expect_x - MY_ABS(imu_t->roll))*dir_x*1.0; 39 40 offset->y_offset = -MY_ABS(expect_y - MY_ABS(imu_t->pitch))*dir_y*1.0; 41 } 42 else 43 { 44 offset->x_offset = 0; 45 offset->y_offset = 0; 46 } 47 48 if(speed>=0) 49 { 50 offset->x_gyro_offset = (speed_x - MY_ABS(imu_t->gyro.x))*dir_x*1.0; 51 offset->y_gyro_offset = (speed_y - MY_ABS(imu_t->gyro.y))*dir_y*1.0; 52 } 53 else if(speed < 0) 54 { 55 offset->x_gyro_offset = -MY_ABS(imu_t->gyro.x)*dir_x*1.0; 56 offset->y_gyro_offset = -MY_ABS(imu_t->gyro.y)*dir_y*1.0; 57 } 58 59 return speed_y; 60 } 61 62 //得到期望角的方向 63 int Get_Direct(int angle, float gyro, float roll, float pitch, int x_y) 64 { 65 int speed = 0; 66 67 speed = gyro*20; 68 if(speed>0) 69 { 70 return 1; 71 } 72 else if(speed == 0) 73 { 74 if(MY_ABS(roll)<3 && MY_ABS(pitch)<3) //判读是否没有动 75 { 76 if(x_y == 0) 77 { 78 if(angle<=90) 79 { 80 return 1; 81 } 82 else 83 { 84 return -1; 85 } 86 } 87 else if(x_y == 1) 88 { 89 return 1; 90 } 91 } 92 return 0; 93 } 94 else if(speed<0) 95 { 96 return -1; 97 } 98 } 99 100 //得到期望的x, y 比例 101 void Get_Expect_X_Y(int angle, float self, float *x, float* y) 102 { 103 float p=0; 104 105 if(self<0) 106 { 107 *x = 0; 108 *y = 0; 109 return; 110 } 111 112 if(angle<90) 113 { 114 p = tan(ANGLE_TO_RADIAN(angle))*1.0; //得到比例值 115 } 116 else if(angle == 90) 117 { 118 *x = 0; 119 *y = self; 120 return ; 121 } 122 else if(angle > 90) 123 { 124 p = tan(ANGLE_TO_RADIAN(angle-90))*1.0; //得到比例值 125 } 126 *x = sqrt(self*self / (1+p*p))*1.0; 127 *y = *x * p; 128 } 129 130 //得到一定角度时的速度 131 //L是杆子的长度 132 float Get_Speed(float length_to_angle, float roll, float pitch, int L) 133 { 134 float now_angle; 135 float h; 136 137 now_angle = sqrt((roll*roll)+(pitch*pitch)); 138 if(now_angle>length_to_angle) 139 { 140 return length_to_angle - now_angle; 141 } 142 h = L*(cos(ANGLE_TO_RADIAN(now_angle))-cos(ANGLE_TO_RADIAN(length_to_angle))); 143 return (sqrt(2*980*h))/L*1.0; 144 } 145 146 147 //===============================================圆锥摆 148 float Get_Circulat_Offset(Imu_t *imu_t, ALL_Expect* expect, ALL_OFFSET* offset) 149 { 150 float r, speed, a; 151 float speed_x, speed_y; 152 float angle; 153 int dir_x = 0, dir_y = 0; 154 155 // dir_x = Get_Direct(expect->angle, imu_t->gyro.x, imu_t->roll, imu_t->pitch, 0); 156 // dir_y = Get_Direct(expect->angle, imu_t->gyro.y, imu_t->roll, imu_t->pitch, 1); 157 Get_Circulat_Dir(imu_t->roll, imu_t->pitch, &dir_x, &dir_y, 1); 158 159 r = 0.7 * sin(ANGLE_TO_RADIAN(expect->length_to_angle)); 160 a = 9.8 * tan(ANGLE_TO_RADIAN(expect->length_to_angle)); //向心加速度 161 speed = sqrt(r * a)*1.0 + 0.31; 162 163 if (imu_t->roll != 0 | imu_t->roll != 180) 164 angle = atan2(MY_ABS(imu_t->pitch), MY_ABS(imu_t->roll)) * 57.3; 165 else 166 angle = 90; 167 Get_Expect_X_Y(angle, speed, &speed_y, &speed_x); 168 169 offset->x_offset = 0; 170 offset->y_offset = 0; 171 offset->x_gyro_offset = (speed_x - MY_ABS(imu_t->gyro.x))*dir_x*1.0; 172 offset->y_gyro_offset = (speed_y - MY_ABS(imu_t->gyro.y))*dir_y*1.0; 173 return speed;; 174 } 175 176 //根据 旋转方向得到 电机方向 177 void Get_Circulat_Dir(float roll, float pitch, int *dir_x, int *dir_y, int dir) // 178 { 179 180 int clock_dir[2][4][2] = { 181 { 182 {+1,-1}, //一相线 183 {+1,+1}, //二相线 184 {-1,+1}, //三相线 185 {-1,-1}, //四相线 186 }, 187 { 188 {-1,+1}, //一相线 189 {-1,-1}, //二相线 190 {+1,-1}, //三相线 191 {+1,+1}, //四相线 192 } 193 } 194 ; 195 //判断象限 196 if(roll>=0 && pitch>=0) 197 { 198 *dir_x = clock_dir[dir][0][0]; 199 *dir_y = clock_dir[dir][0][1]; 200 } 201 else if(roll<0 && pitch>=0) 202 { 203 *dir_x = clock_dir[dir][1][0]; 204 *dir_y = clock_dir[dir][1][1]; 205 } 206 else if(roll<0 && pitch<0) 207 { 208 *dir_x = clock_dir[dir][2][0]; 209 *dir_y = clock_dir[dir][2][1]; 210 } 211 else if(roll>=0 && pitch<0) 212 { 213 *dir_x = clock_dir[dir][3][0]; 214 *dir_y = clock_dir[dir][3][1]; 215 } 216 }
也就简单的两百行
要不你再看看我的PID调节代码，看看简单不简单？？？
1 int P1_1=1, P1_2=1, P1_3=1, P1_4=1; 2 int P2_1=50, P2_2=50, P2_3=50, P2_4=50; 3 void PID_Judge(ALL_OFFSET offset) 4 { 5 uint16_t pwm1=0, pwm2=0, pwm3=0, pwm4=0; 6 7 if(offset.x_offset>=0) 8 { 9 pwm1 += P1_1*offset.x_offset; 10 } 11 else 12 { 13 pwm2 += P1_2*(-offset.x_offset); 14 } 15 ///// 16 if(offset.x_gyro_offset>=0) 17 { 18 pwm1 += P2_1*offset.x_gyro_offset; 19 } 20 else 21 { 22 pwm2 += P2_2*(-offset.x_gyro_offset); 23 } 24 ///// 25 if(offset.y_offset>=0) 26 { 27 pwm3 += P1_3*offset.y_offset; 28 } 29 else 30 { 31 pwm4 += P1_4*(-offset.y_offset); 32 } 33 ///// 34 if(offset.y_gyro_offset>=0) 35 { 36 pwm3 += P2_3*offset.y_gyro_offset; 37 } 38 else 39 { 40 pwm4 += P2_4*(-offset.y_gyro_offset); 41 } 42 43 pwm1 = LIMIT_MAX(pwm1, 80); 44 pwm2 = LIMIT_MAX(pwm2, 80); 45 pwm3 = LIMIT_MAX(pwm3, 80); 46 pwm4 = LIMIT_MAX(pwm4, 80); 47 48 TIM_SetCompare1(TIM10,pwm1); //修改比较值，修改占空比 49 TIM_SetCompare1(TIM11,pwm2); //修改比较值，修改占空比 50 TIM_SetCompare1(TIM13,pwm3); //修改比较值，修改占空比 51 TIM_SetCompare1(TIM14,pwm4); //修改比较值，修改占空比 52 }
总结
这个题目说起来很难，其实想成物理模型就很简单了，也就那样吧
可惜不附上视频不然肯定秀一波，和大神做的差不了多少，照样是乱动 5s 内就能恢复
这个东西加搭硬件写程序只用了2天，因为我不用怎么调PID，全在数学公式。
其实只需要一天半的，但是那个画圆锥的地方出现了问题， 不是公式出了问题，而是想法错了，就是我说的注意的地方，白白焦躁了半天，日了狗，哎。
有问题的可以留言哈
