2013电子设计竞赛-旋转倒立摆-整体方案-2014.10.26更新

Malc

电赛结束了一段时间了，趁这段时间，总结、整理一下自己的方案

------------------------------------------------------------------------------------------------------------

先来看看题目要求吧：



看完题目，经过一番思索和百度之后，有了大概的方案，于是乎开始准备材料，设计机械（我负责），同时其他队员开始设计tb6560驱动、电源模块等等，当然如果你本来就打算选择控制类题目，步进、直流电机及其驱动最好在比赛之前就有充分的准备，我们是使用现成模块的同时开始制作tb6560（比赛的时候其实评委不看我们做的电路，只看我们能否完成题目要求）
用到的材料大部分都是以前的雕刻机项目剩下的材料，倒是省了不少时间，时间宝贵，当然怎么简单有效怎么来了，编码器连接件等东西是这几天加工出来的，abs材料
来张机械结构渲染图，比赛的时候并没有把全部结果画出来，画出个大概就开始做了，那些细节都是比赛后加上去的：

具体的solidworks文件、工程图全部打包在附件中

接下来是电路部分，由于这是控制类题，所以电路比较简单，24v开关电源供电，2596-5v供系统工作，一块arduino nano，一块tb6560，一个ADC键盘用于现场演示，两个mini绝对型编码器1024线
ADC键盘，一个ADC口检测10个按键，我估计检测16个没问题：

tb6560：

2596稳压：


2014.10.26更新：
系统函数图如下：



在接下来就是程序了
总共写了14个版本程序，最后用的就是PID_1_3、PID_1_4，所以这里就给出这两个版本的注释吧：
///////////PID_1_3/////////
PID_1_3.ino :主程序，开机自动检测0°并计算出180°，之后进入循环功能演示。平衡采用PID控制，第一个PID，以摆杆角度作为作为输入，步进电机速度作为输出。第二个PID，以步进速度作为输入，平衡点作为输出（180°±8°）
当摆杆在-110°~-180°或110°~180°范围内时，进行平衡控制，否则关闭步进输出

编码器读数、PID控制放在Timer2中断中，以保证控制周期精准，控制周期5ms
串口命令读取、状态发送放在主函数循环中

起摆思路1：反复震荡摆杆使其摆角越来越大；步进电机往一个方向运动一点距离，停止，等待摆杆达到最高点，再反向运动，停止，等待摆杆达到反向最高点，如此反复，摆杆进入平衡范围后进行平衡控制（起摆时间太长，不采用）

起摆思路2：步进电机突然给一个方向速度，再急刹车，摆杆由于惯性继续圆周运动，当进入平衡范围时，进行平衡控制（时间基本1s以内)

起摆思路3：步进电机突然给一个方向速度，再急刹车，等待摆杆达到最高点，步进电机再反向快速运动，以提高摆杆动能（杆子太重可用此方法)


Stepp.ino :步进电机驱动，使用了Timer1，setStepperSpeed(long myspeed)用于更新速度，StepperEvent()根据速度值驱动步进，如果有加减速效果会更好

Command.ino :接收命令，更新参数，P100代表参数P=100,etc


Encoder.ino :绝对型编码器读数


Filter.ino :FIR低通滤波、中值滤波

function1~6.ino ：分别实现基本要求1~3，提高要求1~3



///////////PID_1_4/////////
PID_1_4.ino : loop中命令值略有更改


Command.ino : 将读取串口数据改为读取ADC键盘值，ADC键盘优点：仅一个AD口可实现10个按键，缺点：读数略复杂，只能单点按键，适合atmega328管脚少的芯片

其他函数同PID_1_3

程序比较长，这里就贴出PID_1_3的主函数吧，完整代码附在附件中：

1. 
   
2. #include <TimerOne.h>
   
3. 
   
4. 
   
5. 
   
6. #define setBit(val, bitn)    (val |=(1<<(bitn)))
   
7. #define clearBit(val, bitn)     (val&=~(1<<(bitn)))
   
8. #define getBit(val, bitn)    (val &(1<<(bitn)) )
   
9. 
   
10. int functionSpeed=0,functionFlag=0;
    
11. int PIDSign=true,SpeedPIDSign=true;;
    
12. int function3Sign=false;
    
13. 
    
14. /////////////Angle Parameter/////////
    
15. long setPoint=0,setPointOpp=0,originPoint=0;
    
16. int degree=0,degreeLast=0,setPointDegree=0;
    
17. 
    
18. 
    
19. /////////Arm Speed Parameter/////////
    
20. #define PULSE_PER_ROUND 1600
    
21. #define MAX_SP_SPEED (PULSE_PER_ROUND*1)
    
22. #define MIN_SP_SPEED (-PULSE_PER_ROUND*1)
    
23. long setPosition=0,position=0,positionLast=0;
    
24. long absPositionLast=0,absPosition=0;
    
25. int setSpeed=0,armSpeed=0,lastSpeed=0;
    
26. int np=500,ni=0,nd=0;
    
27. long sp_error_p=0,sp_error_i=0,sp_error_d=0;
    
28. int tempSpSpeed=0,tempPosition;
    
29. int delta=2,maxDelta=8;
    
30. 
    
31. int calcAngle(int x)
    
32. {
    
33. if(setPointOpp<=x&&x<originPoint)
    
34.   return (x-setPointOpp)*-0.352;
    
35. else if(0<=x&&x<setPointOpp)
    
36.   return (setPointOpp-x)*0.352;
    
37. else
    
38.   return 180-(x-originPoint)*0.352;
    
39.   
    
40. }
    
41. 
    
42. 
    
43. void setup()
    
44. {  
    
45. 
    
46.   EncoderInit();
    
47.   StepperInit();
    
48. 
    
49.   Serial.begin(57600);
    
50.   Serial.print("test begin\ndelay 2s\n");
    
51.   int i,num=5,temp;
    
52.   for(i=0;i<num;i++)
    
53.   {
    
54.     temp=SSI(0);
    
55.     setPointOpp+=temp;//避免编码器0点在摆杆最低点或最高点，简化计算
    
56.     Serial.println(temp);
    
57.     delay(100);
    
58.   }
    
59.   Serial.println(setPointOpp);
    
60.   setPointOpp/=num;
    
61.   Serial.println(setPointOpp);
    
62.   if(setPointOpp<512)
    
63.     setPoint=setPointOpp+512;
    
64.   else
    
65.     setPoint=setPointOpp-512;
    
66.    originPoint=setPoint;
    
67.    setPointDegree=calcAngle(originPoint);
    
68.    Serial.print("setPointOpp=");
    
69.    Serial.println(setPointOpp);
    
70.    Serial.print("setPoint=");
    
71.    Serial.println(setPoint);
    
72.   delay(2000);
    
73.   
    
74.   /*--------------------*/
    
75.   // for Timer2
    
76.   /*--------------------*/
    
77.   // interrupts every 1 ms
    
78.   
    
79.   TIMSK2 &= ~(1<<TOIE2);
    
80.   TCCR2A &= ~((1<<WGM21) | (1<<WGM20));
    
81.   TCCR2B &= ~(1<<WGM22);
    
82.   ASSR &= ~(1<<AS2);
    
83.   TIMSK2 &= ~(1<<OCIE2A);
    
84.   TCCR2B |= (1<<CS22)  | (1<<CS20);
    
85.   TCCR2B &= ~(1<<CS21);
    
86.   TCNT2 = 131;
    
87.   TIMSK2 |= (1<<TOIE2);
    
88.   
    
89.   Timer1.attachInterrupt( StepperEvent ); // attach the service routine here
    
90. 
    
91. }
    
92. 
    
93. 
    
94. int myspeed;
    
95. unsigned long timer;
    
96. 
    
97. int angle,angleLast;
    
98. int error_p,error_i,error_d;
    
99. int velocity;
    
100. 
     
101. int kp=15,kd=0,ki=20;//p=15,i=15 works!!
     
102. 
     
103. unsigned int counter=0;
     
104. 
     
105. 
     
106. void loop()
     
107. {   
     
108.   checkParameter();
     
109.   switch(functionFlag)
     
110.   {
     
111.     case 1:
     
112.            function1();
     
113.            break;
     
114.     case 11:
     
115.            function1();
     
116.            break;
     
117.     case 2:
     
118.            function2();
     
119.            break;
     
120.     case 3:
     
121.            function3();
     
122.            break;
     
123.     case 4:
     
124.            function4();
     
125.            break;
     
126.     case 41:
     
127.            function41();
     
128.            break;
     
129.     case 6:
     
130.            function6();
     
131.            break;
     
132.    
     
133.   }
     
134.    
     
135.   
     
136.   
     
137.   Serial.print(originPoint);
     
138.   Serial.print(',');
     
139.   Serial.print(setPoint);
     
140.   Serial.print(',');
     
141.   Serial.print(angle);
     
142.   Serial.print(',');
     
143.   Serial.print(degree);
     
144.   Serial.print(',');
     
145.   Serial.print(armSpeed);//sp_error_i
     
146.   Serial.print(',');
     
147.   Serial.print(absPosition);
     
148.   Serial.print('\n');
     
149.   
     
150.   //delay(50);
     
151. }
     
152. 
     
153. int calcSpeed(int pos,int posLast)
     
154. {
     
155. if(abs(pos-posLast<512))
     
156.    return pos-posLast;
     
157. int temp;  
     
158. if(pos>posLast)//counter clock
     
159.   temp=1023-pos-posLast;
     
160. else if(pos<posLast)//clock
     
161.   temp=pos+(1023-posLast);
     
162. if(temp>1000)
     
163.    return temp-1023;
     
164. else if(temp<-1000)
     
165.   return 1023+temp;
     
166.   return temp;
     
167. }
     
168. 
     
169. 
     
170. int controlTimer=0,speedTimer=0;;
     
171. int tempPos,tempPosLast;
     
172. int armSpeedArr[3]={0};
     
173. ISR(TIMER2_OVF_vect) {  
     
174.   TIMSK2 |= (0<<TOIE2);
     
175.   TCNT2 = 131;  // reload the timer
     
176.   controlTimer++;
     
177.   speedTimer++;
     
178.   // estimate velocity
     
179.   if (controlTimer == 5){
     
180.      controlTimer = 0;
     
181.      ////////////angle PID///////////
     
182.     angleLast=angle;
     
183.     angle=SSI(0);
     
184.     degreeLast=degree;
     
185.     degree=calcAngle(angle);
     
186.     //angle=FIR2(angle);
     
187.     velocity = (angle - angleLast);
     
188.     if(abs(angle-setPoint)<=4)
     
189.       error_p = 0;
     
190.     else
     
191.       error_p = setPoint-angle;
     
192.     error_d = velocity;
     
193.     error_i = error_i + error_p;
     
194.    
     
195.    if(PIDSign==false||abs(degree)<110)
     
196.   {
     
197.    error_p=0;
     
198.    error_d=0;
     
199.    error_i=0;
     
200.   }
     
201.   myspeed = - (kp*error_p + kd*error_d + ki*error_i/10);
     
202.   setStepperSpeed(myspeed + tempSpSpeed+functionSpeed);
     
203.   }
     
204.   
     
205.   
     
206.   if(speedTimer==5)
     
207.   {
     
208.     speedTimer=0;
     
209.   ////////Speed PID///////////////
     
210.     positionLast=position;
     
211.     lastSpeed=armSpeed;
     
212.     position=SSI(1);
     
213.    
     
214.     armSpeed=calcSpeed(position,positionLast);
     
215.     armSpeedArr[0]=armSpeedArr[1];
     
216.     armSpeedArr[1]=armSpeedArr[2];
     
217.     armSpeedArr[2]=armSpeed;
     
218.     armSpeed=mid(armSpeedArr[0],armSpeedArr[1],armSpeedArr[2]);
     
219.     if(abs(armSpeed)<=1)
     
220.      armSpeed=0;
     
221.    
     
222.     absPositionLast=absPosition;
     
223.     absPosition+=armSpeed;
     
224.    
     
225.     sp_error_p = setSpeed-armSpeed;
     
226.     sp_error_d = armSpeed-lastSpeed;
     
227.     sp_error_i = sp_error_i + sp_error_p;
     
228.     sp_error_i=constrain(sp_error_i,-1000,1000);
     
229.    
     
230.    
     
231.     tempSpSpeed = -(np*sp_error_p/10.0 + nd*sp_error_d/10.0 + ni*sp_error_i/100.0);
     
232.     tempSpSpeed=constrain(tempSpSpeed,-delta,delta);
     
233.     //if(PIDSign==false||SpeedPIDSign==false||abs(degree)>155)
     
234.     if(PIDSign==true&&SpeedPIDSign==true&&abs(degree)>155)
     
235.     {
     
236.       setPoint+=tempSpSpeed;
     
237.       if(setPoint>originPoint+maxDelta)
     
238.        setPoint=originPoint+maxDelta;
     
239.       else if(setPoint<originPoint-maxDelta)
     
240.        setPoint=originPoint-maxDelta;
     
241.     }
     
242.     else if(SpeedPIDSign==true)
     
243.     {
     
244.       setPoint=originPoint;
     
245.     }
     
246.     }
     
247.    
     
248.     //setStepperSpeed(myspeed + tempSpSpeed);
     
249. 
     
250.   TIMSK2 |= (1<<TOIE2);
     
251. }
     

复制代码

视频、图片在这里：



附件：




115：
http://115.com/lb/5lbawy4ygq2#
倒立摆1.rar
115网盘礼包码：5lbawy4ygq2

TB6560 资料：

Malc

gyq 发表于 2014-8-4 10:40
你说的用502固定在步进电机的轴上么，还是底座上，要是在底座上的话，编码器怎么采集转轴的速度数据呀，这 ...

用502来粘合步进电机的轴与编码器的轴，用胶带固定编码器，防止自转

弘毅

{:soso_e102:}终于看到源码了。。。仔细学习

幻生幻灭

这效果图让我想起了那个小蜘蛛

Malc

弘毅 发表于 2013-10-7 21:04
终于看到源码了。。。仔细学习

时间紧迫，代码写的比较乱，算法有待改进。。
ps：那两个nano偷偷拆下来自己用了{:soso_e113:}

Malc

幻生幻灭 发表于 2013-10-7 21:15
这效果图让我想起了那个小蜘蛛

不玩蜘蛛了，雕刻机更好玩~

ZYLLOVE

关注你很久了，好喜欢你的文章，终于知道你的倒立摆是咋做的的啦~学习，谢谢~~~

lmb312

Uno的话程序能用么

小黑爱GEEK

我们做的也是这个，不过跟您的比起来简直就是个渣啊，这会真的学习了，膜拜啊

tangmao48

这架势应该有国一吧。我们的学生一组国一一组国二。没有您这位大神做得好。

惠普彩色打印机

大神。。。。肯定是国一，一个地区三等奖给跪了

惠普彩色打印机

做D题的孩纸快要被吓死了

wisology

编码器放在哪儿了？硬件及机械部分是不是没讲完整，我个人理解是放到倒立摆转轴那块了。

Malc

wisology 发表于 2013-10-11 21:52
编码器放在哪儿了？硬件及机械部分是不是没讲完整，我个人理解是放到倒立摆转轴那块了。

两个编码器，一个装在摆臂末端，检测摆杆的转角，另一个装在步进底下，步进没有闭环的话摆杆会一直在水平方向移动停不下来，跟平衡车一样

龙翔竞天

这个厉害。。。。。想都没想过。。

青青木麻黄

太赞了！！！