标题: 风力摆控制系统赛题解析与操作说明及STM32源码(重要的是PID算法) [打印本页]

---

作者: 陈某    时间: 2018-8-8 16:30
标题: 风力摆控制系统赛题解析与操作说明及STM32源码(重要的是PID算法)
电子竞赛题目《风力摆控制系统》
本人的课程设计；
重要的是PID算法。



没有电路图。

电机的控制：电机1  控制正反转   ->PD14,PD12      PWM波 -> PA1
        电机2  控制正反转   ->PD15,PD13      PWM波 -> PA0
            电机3  控制正反转   ->PD11,PD10      PWM波 -> PA3
            电机4  控制正反转   ->PD9,PD8        PWM波 -> PA2

MPU6050    SDA ->PB7     SCL ->PB6

按键   K1-> PA4, K2->PA5, K3->PA6,  K4->PA7.


操作：K3为开始键  ；K4为选择模式；K1画圆时增加半径，画直线时增加长度；K2画圆时减小半径，画直线时减小长度；
      按一下K4为画直线，按两下是长度可控的直线，按下是角度可变的直线，按四下是5S静止，按五下是画圆

风力摆控制系统赛题解析
选型方案
第一步，控制系统选型：刚才说了，我们需要做一个伺服随动控制系统，通常衡量伺服系统性能的指标有“带宽”、“精度”、“抗干扰能力”等。先说系统带宽，带宽反映风力摆跟踪的快速性。带宽越大，快速性越好。风力摆控制系统的带宽主要受到控制对象和执行机构的惯性的限制。惯性越大，带宽越窄。根据题目要求做“自由摆运动”，由单摆周期公式可以求得风力摆的摆动周期 T 在 1.3-1.6 秒之间，换算成频率 f 在 0.625-0.77Hz 之间，大家可以自己掐表计算一下摆动的周期。由此可知我们需要设计出一个带宽大于 0.77Hz 的控制系统（取0.8Hz）即可完成题目的要求。出题的专家制定的这个指标还是简单的，要知道现代伺服控
制系统带宽已经超过了 50Hz。角度采样率根据奈奎斯特采样定理，理论上选取 fs>2f 即可但是题目中要求了系统最大调节时间，为了使得控制效果更好，需要取 fs>10f 甚至更高，在本次设计中采样率选取 200Sa/s，控制周期 5ms。
第二步，电机选型：这是本题的最大争议，在此我也不想把这个争议扩大，所以我不讨论电机是否违规的问题。现在从实现效果这个角度来讨论电机的选型，不管黑猫白猫，抓到老鼠就是好猫！
我把轴流电机、空心杯电机、无刷电机做成一个表格。


主要看推重比和机动性这一栏，推重比、机动性是航空器的专业术语，发动机在水平面上的最大推力和发动机的净重之比称为推重比。机动性是指风力摆在一定时间内改变运动速度、方向的能力。从这个表格来看，推重比是我们选择电机的重要指标，而机动性是整个风力摆灵活性的重要指标。注意到赛题目说明部分的第 9 条：“赛题中要求的各项动作完成时间越短越好”。毫无疑问，出题人已经说明了要尽可能的提高系统的机动性。从最优实现效果来看，无刷电机和空心杯电机是更优的选择。但是为什么有人用轴流电机也能完成题目要求呢？答案就在频率响应上，第一步已经分析了系统带宽 0.8Hz 即可满足题目要求。如果题目规定 15 秒内完成20 个单摆运动，这个难度就上来了，因为用轴流电机的系统带宽不够了。但是采用轴流电机有个好处，在圆周运动时即使做开环控制风扇干扰几乎吹不动。第三步，传感器选型：我们再说说精度（检测误差），检测误差包括传感器的误差和机械误差，是传感器和机械本身所固有的，控制系统无法克服。根据题目给出的±2.5cm 偏差可以知传感器精度只要达到 0.5°就可以了，所以用 MPU6050 传感器是可以达到题目要求的。机械误差则要求机械完美的对称，所有的东西都要对称，机械做的越好，最后出来的效果就越好。至于传感器的数据融合、滤波器设计部分请看程序贴图：计算三次角度然后求平均值，再经过卡尔曼滤波器滤波的角度值就可以使用了。角度数据一定要稳定，无高频干扰！

（如果图片中的字看不清可以下载附件直接看）

算法设计
当硬件 OK 以后算法就是整个系统的核心灵魂了，俗话说条条大路通罗马，能够达到题目要求的方法有很多，这次比赛童鞋们用的算法大概有几种方式：逻辑判断法、查表法，定点PID 法，力合成法，矢量方程法等等。有的队直接采用查表法开环控制也能取得不错的成绩，只要能够达到要求的算法都是 OK 的。
说一下我的算法设计吧，出题人在第一问就告诉了要做自由摆运动，从自由摆可以联想到2011 年的 B 题——基于自由摆的平板控制系统。这是一个单摆，但是单摆是非线性运动，我们通常会把非线性的东西通过某种思路去近似成线性的（如二极管的伏安特性曲线等效模型），在单摆中，当采样率足够高时两个采样点之间的连线可以近似看成是线性的,有木有感到很熟悉？在高中物理学过了简谐运动，物体所受的力跟位移成正比，并且总是指向平衡位置。这是一种由自身系统性质决定的周期性运动。（如单摆运动和弹簧振子运动）实际上简谐振动就是正弦振动！其数学方程为θ(t)=Asin(ωt+ψ)，A 是振幅，ω是频率，ψ是相位。好！说到这，来直接看一下题目第二问的要求（第一问直接跳过）。题目要求幅度可控，幅度可控是啥？不就是振幅 A 可控嘛，可控范围是多少？30-60cm 换成角度。好了，分析完了，第二问就是一个 A 可设置的正弦运动，OK。至于线性度偏差，只要你机械搭对称了，这都不是问题，况且别忘了有 X,Y 两个方向的电机呢。
把第二问的程序贴图出来：


第三问，摆动方向可设置。这里需要介绍一个知识点了：李萨茹图形。当风力摆同时参与两个相互垂直方向的简谐振动,风力摆的位移是这两个振动的位移的矢量和,如果两个振动的频率具有整数比关系时,风力摆的运动路径是稳定的封闭的曲线，这些曲线即李萨如图形。具体的理论推导和计算这里就不贴出来了，可以自行百度，现在直接把结论贴出来，李萨茹图形由以下参数方程定义：
x(θ)=Asinθ
y(θ)=Bsin(θ+ψ)
若 A=B，ψ=任意，则曲线是椭圆；


直接看图清晰明了，第三问我们只需要在 X,Y 方向分别进行频率相同，相位ψ=0 或π，振幅
A,B 可设置的简谐运动即可很好的完成题目的要求。
例如，想要 45°怎么办？tan45=1，设置振幅 A/B=1 即可：
x(θ)=10*sinθ
y(θ)=10*sinθ
例如，想要 60°怎么办？tan60=根号 3，设置振幅 A/B=根号 3 即可：
x(θ)=根号 3*sinθ
y(θ)= 1*sinθ
例如，想要 120°怎么办？tan120=-根号 3，设置振幅 A/B=-根号 3 即可：
x(θ)=-根号 3*sinθ
y(θ)= 1*sinθ
细心的童鞋一定会发现，出题人对第三问的要求简单了，只要求摆动角度可控，但是通过上
述方法不仅角度可控，摆长也是可控的！

把第三问的程序贴图出来：


第四问，5 秒内能够制动。这个没有什么运动路径可言，纯粹是考察大家的 PID 是否熟练掌握，PID 目标值设为 0°即可。注意 D 要给大一些，不要有积分。
第四问的程序图：



第五问，画圆。出题人通过基础部分一步一步引导完成画圆，真是用心良苦。如果能够领
会出题人的想法，把第二问、第三问完成好，这一问是水到渠成的事情。再次把第三问的图
贴出来，如何画圆？
若 A=B，ψ=π/2 或 3π/2，则曲线是圆！
OK！完成了，这是圆，而且画出来很圆！如果画出了椭圆，一定是相位ψ有偏差，调节一下
相位即可！
第五问的程序贴图：


细心的童鞋会发现，题目做到这一步，θ(t)=Asin(ωt+ψ)这个方程就是贯穿本题的核心！如果你的系统做得足够好，就会发现这个方程的每一个参数你都可以自由的控制，结果就是你不仅可以让摆幅可控、摆动周期可控、摆动方向可控，而且可以画出以下这些曲线：




第六问：画圆抗干扰。考察伺服系统的抗扰动能力，出题人用这一问来把开环，半开环控制和闭环控制的队伍拉开距离。当一个干扰过来的时候，风力摆的运动方程肯定不是θ(t)=Asin(ωt+ψ)了，题目给出了系统最大调节时间为 5 秒，能够在 5 秒内恢复 Asin(ωt+ψ)即可。这就牵扯到了伺服控制系统的重要指标——快速跟踪和准确定位。有很多种算法可以选择，例如常见的 LQR、自适应、模糊控制、神经网络等等。相信绝大部分还是用了经典的 PID 算法，简单，快速，效果尚可！就看谁的PID调得熟练了，这个需要功夫在平时，多调多想多看就 OK！PID 需要调成大概是这样的波形，能够在一个周期之内快速跟踪正弦运动：


如果调成这样，波形相似相位跟不上，系统惯性大，请把 D 调高。


第七问：其他部分，今年出题人给其他项分配了 10 分，想拿高分的队伍是必须拿下这 10分的。前面说了，如果你的控制器做得足够好，是能够画出花样百出的图形的，有很大的发挥余地。有的队伍是画三角形、正方形、画 8 字，有的是做跟踪物体、手写写字、画斜直线，做漂亮的 GUI 界面等等都有。根据以往经验，在有限的时间内建议不要把时间花在漂亮的GUI 界面上，要放在更多的运动控制上，牢牢把握题目考察的侧重点拿分！总的来看，今年这道题目难度适中，出题人还是考虑得比较周全的，让各种风机各种算法都能够达到要求，众口难调，确实不简单。同时也反映了电赛控制类的趋势。从 2011 年的小车这种纯逻辑判断的题目到 2013 年倒立摆这种定点稳态系统再到 2015 年风力摆的动态跟踪系统，实现了从逻辑到算法、从静态到动态、从二维到三维的转变，准备 2017 年电赛控制类的同学要多多留心了。个人感觉功夫还需在平时，搞突击是不行的，多参加比赛，多接触实际的项目好处多多，机会留给有准备的人，相信你也可以！

stm32单片机源程序如下:

1. 
   
2. #include "public.h"
   
3. #include "systick.h"
   
4. #include "mpu6050.h"
   
5. #include "iic_analog.h"
   
6. #include "indkey.h"
   
7. #include "Time_Key.h"
   
8. #include "Beep.h"
   
9. #include "moto_PWM.h"
   
10. #include "Data.h"
    
11. #include "ahrs.h"
    
12. #include "pwm.h"
    
13. #include "usart2.h"
    
14. 
    
15. extern uint8_t Q1_Start;
    
16. extern uint8_t Q2_Start;
    
17. extern uint8_t Q3_Start;
    
18. extern uint8_t Q4_Start;
    
19. extern uint8_t Q5_Start;
    
20. extern uint8_t Q6_Start;
    
21. extern uint8_t CurMode;
    
22. //void gpio_init(void);
    
23. 
    
24. void BSP_Init(void)
    
25. {         
    
26.   //static u8 max,max1;
    
27.         
    
28.         SystemInit();
    
29.         pwm_init();         //PWM初始化
    
30.         //gpio_init();
    
31.         Key_IO_Init();
    
32.         MPU6050_Init();
    
33.         //delay_ms(500);
    
34.         //while(MPU_Init()==0);
    
35.         TIM5_Config(5000-1,71);
    
36.         TIM1_Config(10000-1,71);
    
37.         TIM_Cmd(TIM1,ENABLE);            
    
38.         TIM_Cmd(TIM5,ENABLE);
    
39.         USART2_Config();
    
40.         Display_Title();
    
41.         //GPIO_SetBits(GPIOB,GPIO_Pin_7);
    
42.         
    
43.         moto_control();
    
44.         //GPIO_ResetBits(GPIOB,GPIO_Pin_7);
    
45.         //moto3_turn(500,1);
    
46. }
    
47. int main(void)
    
48.         
    
49. {  
    
50.         static u8 max,max1;
    
51.         BSP_Init();
    
52.         while(1)
    
53.         {
    
54.    
    
55.                
    
56.                 if(Q1_Start == 1)
    
57.                 {
    
58.                         CurMode = 1;
    
59.                 }
    
60.                 else if(Q2_Start == 1)
    
61.                 {
    
62.                         CurMode = 2;
    
63.                 }
    
64.                 else if(Q3_Start == 1)
    
65.                 {
    
66.                          CurMode = 3;
    
67.                 }
    
68.                 else if(Q4_Start == 1)
    
69.                 {
    
70.                          CurMode = 4;
    
71.                 }
    
72.                 else if(Q5_Start == 1)
    
73.                 {
    
74.                         CurMode = 5;
    
75.                 }
    
76.                
    
77.                 else
    
78.                 {
    
79.                         CurMode = 0;
    
80.                 }        
    
81.                
    
82.                 if(PID_data.erro>max)
    
83.                 max=PID_data.erro;
    
84. 
    
85.                                 if(PID_data1.erro>max1)
    
86.                     max1=PID_data1.erro;
    
87.                         /*if(key_board())
    
88.                                 key_nuber=key_board();*/
    
89.                         //delay_ms(200);
    
90.                
    
91.         }
    
92. }
    
93. 
    
94. 
    
95. 
    
96. 
    
97. //void gpio_init(void)
    
98. //{
    
99. //        GPIO_InitTypeDef GPIO_InitStructure;        //声明一个结构体变量，用来初始化GPIO
    
100. 
     
101. //        SystemInit();
     
102. //        RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);   /*开启GPIO时钟*/
     
103. //        /*  配置GPIO的模式和IO口 */
     
104. //        GPIO_InitStructure.GPIO_Pin=GPIO_Pin_7;                //选择你要设置的IO口
     
105. //        GPIO_InitStructure.GPIO_Mode=GPIO_Mode_Out_PP;                  //设置推挽输出模式
     
106. //        GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;          //设置传输速率
     
107. //        GPIO_Init(GPIOB,&GPIO_InitStructure);          /* 初始化GPIO */
     
108. //}
     
109. 
     
110. 
     

复制代码

1. #include "Key.h"
   
2. #include "stm32f10x.h"
   
3. #include "systick.h"
   
4. uint8_t Item = 0;
   
5. 
   
6. uint8_t Q1_Start = 0;
   
7. uint8_t Q2_Start = 0;
   
8. uint8_t Q3_Start = 0;
   
9. uint8_t Q4_Start = 0;
   
10. uint8_t Q5_Start = 0;
    
11. uint8_t Q6_Start = 0;
    
12. uint8_t Q7_Start = 0;
    
13. extern char buf[512];         //液晶屏数据缓冲区
    
14. extern float R;
    
15. extern float angle;
    
16. extern uint8_t RoundDir;
    
17. extern uint8_t CurMode;
    
18. void Key_GPIO_Init(void)
    
19. {
    
20.         GPIO_InitTypeDef GPIO_InitStructure;        //声明一个结构体变量，用来初始化GPIO
    
21.         
    
22.         RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);   /* 开启GPIO时钟 */
    
23.         //RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);   /* 开启GPIO时钟 */
    
24.         SystemInit();
    
25.         /*  配置GPIO的模式和IO口 */
    
26.         GPIO_InitStructure.GPIO_Pin=GPIO_Pin_4|GPIO_Pin_5|GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_14|GPIO_Pin_15;                //选择你要设置的IO口
    
27.         GPIO_InitStructure.GPIO_Mode=GPIO_Mode_IPU;                  //设置推挽输出模式
    
28.         GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;          //设置传输速率
    
29.         GPIO_Init(GPIOA,&GPIO_InitStructure);          /* 初始化GPIO */
    
30.         
    
31. //        GPIO_InitStructure.GPIO_Pin=GPIO_Pin_10|GPIO_Pin_11;
    
32. //        GPIO_InitStructure.GPIO_Mode=GPIO_Mode_IPU;                  //设置推挽输出模式
    
33. //        GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;          //设置传输速率
    
34. //        GPIO_Init(GPIOB,&GPIO_InitStructure);
    
35. }
    
36. 
    
37. //u8 Key_Scan(GPIO_TypeDef* GPIO,uint16_t Pin)
    
38. //{
    
39. //                if(GPIO_ReadInputDataBit(GPIO,Pin)==0)
    
40. //                {
    
41. //                        delay_ms(20);
    
42. //                        if(GPIO_ReadInputDataBit(GPIO,Pin)==0)
    
43. //                        {
    
44. //                                while (GPIO_ReadInputDataBit(GPIO,Pin)==0);
    
45. //                                return 1;
    
46. //                        }
    
47. //                }
    
48. //                return 0;
    
49. //}
    
50. 
    
51. 
    
52. //u8 key_board(void)
    
53. //{
    
54. //        u8 key=0;
    
55. //        key=Key_Scan(GPIOA,GPIO_Pin_4)+
    
56. //                        Key_Scan(GPIOA,GPIO_Pin_5)*2+
    
57. //                        Key_Scan(GPIOA,GPIO_Pin_6)*3+
    
58. //                        Key_Scan(GPIOA,GPIO_Pin_7)*4+
    
59. //                        Key_Scan(GPIOA,GPIO_Pin_14)*5+
    
60. //                        Key_Scan(GPIOA,GPIO_Pin_15)*6+
    
61. //                        Key_Scan(GPIOB,GPIO_Pin_10)*7+
    
62. //                        Key_Scan(GPIOB,GPIO_Pin_11)*8;
    
63. //        return key;
    
64. //}
    
65. void KeyScan(void)
    
66. {        
    
67.         if(GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_4) == 0) //K1
    
68.         {        
    
69.                 switch(Item)
    
70.                 {
    
71.                         case 2:R+=5.0;
    
72.                                    if(R >= 35.0) R = 35.0;
    
73. //                                   sprintf(buf,"DS16(6,60,'设置长度:%.1f ',10)\r\n",R);
    
74. //                                   GpuSend(buf);        
    
75.                                break;  //第2问按下S4增加距离
    
76. 
    
77.                         case 3:angle+=10.0;
    
78.                                    if(angle >= 180.0)
    
79.                                              angle = 180.0;
    
80. //                                   sprintf(buf,"DS16(6,80,'设置角度:%.1f ',10)\r\n",angle);
    
81. //                                   GpuSend(buf);        
    
82.                                break;  //第3问按下S4增加角度;  
    
83.                         
    
84.                         case 5:R+=5.0;
    
85.                                    if(R >= 35.0) R = 35.0;
    
86. //                                   sprintf(buf,"DS16(6,100,'设置半径:%.1f ',10)\r\n",R);
    
87. //                                   GpuSend(buf);
    
88.                                    break;
    
89. 
    
90.                         case 6:R+=5.0;
    
91.                                    if(R >= 35.0) R = 35.0;
    
92. //                                   sprintf(buf,"DS16(6,100,'设置半径:%.1f ',10)\r\n",R);
    
93. //                                   GpuSend(buf);
    
94.                                    break;
    
95.                                    
    
96. 
    
97.                         default:break;
    
98.                 }                                
    
99.         }
    
100. 
     
101.         if(GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_5) == 0) //K2
     
102.         {
     
103.                 switch(Item)
     
104.                 {
     
105.                         case 2:R-=5.0;
     
106.                                    if(R <= 15.0) R = 15.0;
     
107. //                                   sprintf(buf,"DS16(6,60,'设置长度:%.1f ',10)\r\n",R);
     
108. //                                   GpuSend(buf);        
     
109.                                break;  //第2问按下S4增加距离
     
110. 
     
111.                         case 3:angle-=10.0;
     
112.                                    if(angle <= 0.0)
     
113.                                              angle = 0.0;
     
114. //                                   sprintf(buf,"DS16(6,80,'设置角度:%.1f ',10)\r\n",angle);
     
115. //                                   GpuSend(buf);        
     
116.                                break;  //第3问按下S4增加角度;  
     
117.                         
     
118.                         case 5:R-=5.0;
     
119.                                    if(R <= 15.0) R = 15.0;
     
120. //                                   sprintf(buf,"DS16(6,100,'设置半径:%.1f ',10)\r\n",R);
     
121. //                                   GpuSend(buf);
     
122.                                    break;
     
123. 
     
124.                         case 6:R-=5.0;
     
125.                                    if(R <= 15.0) R = 15.0;
     
126. //                                   sprintf(buf,"DS16(6,100,'设置半径:%.1f ',10)\r\n",R);
     
127. //                                   GpuSend(buf);
     
128.                                    break;
     
129.                                    
     
130. 
     
131.                         default:break;
     
132.                 }               
     
133.         }
     
134.         
     
135.         if(GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_6) == 0) //K3
     
136.         {
     
137.                 switch(Item)
     
138.                 {
     
139.                         case 1:Q1_Start = 1;
     
140. //                                   sprintf(buf,"DS16(6,120,'开始!',10)\r\n");
     
141. //                                   GpuSend(buf);
     
142.                                    break;
     
143. 
     
144.                         case 2:Q2_Start = 1;
     
145.                                    //sprintf(buf,"DS16(6,120,'开始!',10)\r\n");
     
146.                                    //GpuSend(buf);
     
147.                                    break;
     
148. 
     
149.                         case 3:Q3_Start = 1;
     
150.                                   // sprintf(buf,"DS16(6,120,'开始!',10)\r\n");
     
151.                                    //GpuSend(buf);
     
152.                                    break;  
     
153. 
     
154.                         case 4:Q4_Start = 1;
     
155.                                   // sprintf(buf,"DS16(6,120,'开始!',10)\r\n");
     
156.                                    //GpuSend(buf);
     
157.                         break;
     
158.                                    
     
159. //                        case 5:Q5_Start = 1;
     
160. //                                   RoundDir = !RoundDir;
     
161. //                                   if(RoundDir == 1)
     
162. //                                         // sprintf(buf,"DS16(6,120,'顺时针旋转!',10)\r\n");
     
163. //                                   else
     
164. //                                              sprintf(buf,"DS16(6,120,'逆时针旋转!',10)\r\n");
     
165. //                                   GpuSend(buf);break;
     
166.                                    
     
167.                         case 6:Q6_Start = 1;               
     
168.                               // sprintf(buf,"DS16(6,120,'开始!',10)\r\n");
     
169.                                   // GpuSend(buf);
     
170.                                         break;   
     
171. 
     
172.                         default:break;
     
173.                 }        
     
174.         }         
     
175. 
     
176.         if(GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_7) == 0)  //K4
     
177.         {
     
178.                 Item++;
     
179.                 if(Item>6)                //共有7问
     
180.                         Item = 0;
     
181.                 //sprintf(buf,"DS16(6,40,'第%d问',10)\r\n",Item);
     
182.                 //GpuSend(buf);
     
183.         }
     
184. }
     
185. 
     
186. 
     

复制代码

所有资料51hei提供下载:
风力摆控制系统+操作说明.rar (1.09 MB, 下载次数: 329)

2018-8-9 01:54 上传

点击文件名下载附件
下载积分: 黑币 -5

---

作者: admin    时间: 2018-8-9 01:56
好资料，51黑有你更精彩!!!

---

作者: wangxianlong    时间: 2018-8-11 20:52
谢谢楼主

---

作者: lxh666    时间: 2019-7-10 20:08
压缩包解压不了，显示压缩包损坏或文件格式未知。
这是怎么回事？

---

作者: 51hei团团    时间: 2019-7-10 21:43

lxh666 发表于 2019-7-10 20:08
压缩包解压不了，显示压缩包损坏或文件格式未知。
这是怎么回事？

貌似我用winrar可以解压

---

作者: 18239311863    时间: 2019-7-25 23:24
好资料，正好学习

---

作者: 深山小妖    时间: 2019-7-26 10:57
膜拜大佬

---

作者: 堂堂    时间: 2019-7-28 22:39
膜拜大佬

---

作者: WD51H    时间: 2019-7-31 14:57
MPU6050有姿态融合吗？数据漂移情况如何？

---

作者: 迁就    时间: 2019-8-1 09:25
谢谢楼主分享

---

作者: 迁就    时间: 2019-8-1 09:37
谢谢楼主分享

---

作者: 自学成才1    时间: 2019-8-2 11:07
资料是好资料，但是希望你能够标明你是转载的，我正好刚看过原作者的帖子

---

作者: a1111112222    时间: 2019-8-3 17:14
感谢分享

---

作者: a1721551515    时间: 2019-8-3 18:03
厉害了楼主

---

作者: luchenzhijia    时间: 2020-9-20 17:54
加油，不错！

---

作者: 可乐我要柠檬味    时间: 2021-3-14 14:58
为什么压缩包里面的程序没有看到PID部分哇

---

作者: 51hei团团    时间: 2021-3-14 15:21

可乐我要柠檬味 发表于 2021-3-14 14:58
为什么压缩包里面的程序没有看到PID部分哇

是这个吗Subject.c

1. #include "Subject.h"
   
2. #include "Data.h"
   
3. #include "math.h"
   
4. #include "moto_PWM.h"
   
5. #include "math.h"
   
6. #define H    79.0                  //万向节距离地面的高度
   
7. float R=25.0;           //摆动弧度的大小半径
   
8. #define T    1410.0   //摆动的周期
   
9. float angle = 45.0;
   
10. 
    
11. extern Data  PID_data;
    
12. extern Data  PID_data1;
    
13. uint8_t RoundDir=1;
    
14. //const float Phase;
    
15. void Subject_1(void)
    
16. {
    
17.         float A=0.0;
    
18.         static uint32_t MoveTimeCnt = 0;
    
19.         float set_y = 0.0;
    
20.         float Normalization = 0.0;
    
21.         float Omega = 0.0;
    
22.         MoveTimeCnt+=5;
    
23.         Normalization=(float)MoveTimeCnt/T;
    
24.         Omega=2*3.1415926*Normalization;      //求一个角度 随时间变化的 每5毫秒加2*PI*5/T度   来求相位
    
25.        
    
26.         A=atan((R/79.0f))*57.2958f;                                                //最大角度 摆的最大高度
    
27.        
    
28.         set_y=A*sin(Omega);                        //轨迹函数
    
29.        
    
30.         PID_data.setPoint=0;
    
31.         PID_data.P=170;
    
32.         PID_data.I=5.8;
    
33.         PID_data.D=500;
    
34.        
    
35.        
    
36.         PID_data1.setPoint=set_y;
    
37.         PID_data1.P=150;
    
38.         PID_data1.I=6.8;
    
39.         PID_data1.D=500;
    
40.        
    
41.         PID_data.PWM=PID_Calc1();
    
42.         PID_data1.PWM=PID_Calc2();
    
43.        
    
44.         if(PID_data.PWM>1500)
    
45.                 PID_data.PWM=1500;
    
46.         /*else if(PID_data.PWM>0&&PID_data.PWM<450)
    
47.                 PID_data.PWM=450;*/
    
48.         if(PID_data.PWM<-1500)
    
49.                 PID_data.PWM=-1500;
    
50.                 /*else if(PID_data.PWM<0&&PID_data.PWM>-450)
    
51.                 PID_data.PWM=-450;*/
    
52.                
    
53.         if(PID_data1.PWM>1500)
    
54.                 PID_data1.PWM=1500;
    
55.         if(PID_data1.PWM<-1500)
    
56.                 PID_data1.PWM=-1500;
    
57.        
    
58.         moto_PWM(PID_data.PWM,PID_data1.PWM);
    
59.        
    
60. }
    
61. 
    
62. 
    
63. 
    
64. void Subject_2(void)
    
65. {
    
66.         float A=0.0;
    
67.         static uint32_t MoveTimeCnt = 0;
    
68.         float set_x = 0.0;
    
69.         float Normalization = 0.0;
    
70.         float Omega = 0.0;
    
71.         MoveTimeCnt+=5;
    
72.         Normalization=(float)MoveTimeCnt/T;
    
73.         Omega=2*3.1415926*Normalization;
    
74.         A=atan((R/79.0f))*57.2958f;
    
75.         set_x=A*sin(Omega);
    
76.        
    
77.         PID_data.setPoint=set_x;
    
78.         PID_data.P=191;
    
79.         PID_data.I=5.9;
    
80.         PID_data.D=420;
    
81.        
    
82.        
    
83.         PID_data1.setPoint=0;
    
84.         PID_data1.P=100;
    
85.         PID_data1.I=5.8;
    
86.         PID_data1.D=500;
    
87.        
    
88.         PID_data.PWM=PID_Calc1();
    
89.         PID_data1.PWM=PID_Calc2();
    
90.        
    
91.         if(PID_data.PWM>1500)
    
92.                 PID_data.PWM=1500;
    
93.         if(PID_data.PWM<-1500)
    
94.                 PID_data.PWM=-1500;
    
95.        
    
96.         if(PID_data1.PWM>1500)
    
97.                 PID_data1.PWM=1500;
    
98.         if(PID_data1.PWM<-1500)
    
99.                 PID_data1.PWM=-1500;
    
100.        
     
101.         moto_PWM(PID_data.PWM,PID_data1.PWM);
     
102.        
     
103. }
     
104. 
     
105. 
     
106. void Subject_3(void)
     
107. {
     
108.         //const float priod = 1410.0;  //单摆周期(毫秒)
     
109.                      //相位补偿 0, 10   20   30   40   50   60   70   80   90   100  110  120  130  140  150  160  170 180
     
110.         const float Phase[19]= {0,-0.1,-0.05,0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,0.05,0.05,0.05,0.07,0};
     
111.         static uint32_t MoveTimeCnt = 0;
     
112.         float set_x = 0.0;
     
113.         float set_y = 0.0;
     
114.         float Ax = 0.0;
     
115.         float Ay = 0.0;
     
116.         float A = 0.0;
     
117.         uint32_t pOffset = 0;
     
118.         float Normalization = 0.0;
     
119.         float Omega = 0.0;
     
120.        
     
121.         pOffset = (uint32_t)(angle/10.0f);                         //相位补偿数组下标
     
122.         MoveTimeCnt += 5;                                                         //每5ms运算1次
     
123.         Normalization = (float)MoveTimeCnt/T;         //对单摆周期归一化
     
124.         Omega = 2.0*3.14159*Normalization;                         //对2π进行归一化处理
     
125.         A = atan((R/79.0f))*57.2958f;//根据摆幅求出角度A,88为摆杆离地高度                                                                  
     
126.         Ax = A*cos(angle*0.017453);         //计算出X方向摆幅分量0.017453为弧度转换
     
127.         Ay = A*sin(angle*0.017453);         //计算出Y方向摆幅分量
     
128.         set_x = Ax*sin(Omega);                  //计算出X方向当前摆角
     
129.         set_y = Ay*sin(Omega+Phase[pOffset]); //计算出Y方向当前摆角
     
130.                
     
131.         PID_data.setPoint=set_y;
     
132.         PID_data.P=180;
     
133.         PID_data.I=6.5;
     
134.         PID_data.D=500;
     
135.        
     
136.        
     
137.         PID_data1.setPoint=set_x;
     
138.         PID_data1.P=110;
     
139.         PID_data1.I=5.9;
     
140.         PID_data1.D=500;         
     
141.        
     
142.         PID_data.PWM=PID_Calc1();
     
143.         PID_data1.PWM=PID_Calc2();
     
144.        
     
145.         if(PID_data.PWM>1700)
     
146.                 PID_data.PWM=1700;
     
147. 
     
148.         if(PID_data.PWM<-1700)
     
149.                 PID_data.PWM=-1700;
     
150. 
     
151.        
     
152.         if(PID_data1.PWM>1700)
     
153.                 PID_data1.PWM=1700;
     
154. 
     
155.         if(PID_data1.PWM<-1700)
     
156.                 PID_data1.PWM=-1700;
     
157. 
     
158.         moto_PWM(PID_data.PWM,PID_data1.PWM);
     
159. }
     

复制代码

---

欢迎光临 (http://www.51hei.com/bbs/)

Powered by Discuz! X3.1