先来看看题目要求吧:
看
完题目,经过一番思索和百度之后,有了大概的方案,于是乎开始准备材料,设计机械(我负责),同时其他队员开始设计tb6560驱动、电源模块等等,当然
如果你本来就打算选择控制类题目,步进、直流电机及其驱动最好在比赛之前就有充分的准备,我们是使用现成模块的同时开始制作tb6560(比赛的时候其实
评委不看我们做的电路,只看我们能否完成题目要求)
用到的材料大部分都是以前的雕刻机项目剩下的材料,倒是省了不少时间,时间宝贵,当然怎么简单有效怎么来了,编码器连接件等东西是这几天加工出来的,abs材料
来张机械结构渲染图,比赛的时候并没有把全部结果画出来,画出个大概就开始做了,那些细节都是比赛后加上去的:
具体的solidworks文件、工程图全部打包在附件中
接下来是电路部分,由于这是控制类题,所以电路比较简单,24v开关电源供电,2596-5v供系统工作,一块arduino nano,一块tb6560,一个ADC键盘用于现场演示,两个mini绝对型编码器1024线
ADC键盘,一个ADC口检测10个按键,我估计检测16个没问题:
tb6560:
2596稳压:
在接下来就是程序了
总共写了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的主函数吧,完整代码附在附件中:
ARDUINO 代码
01.#include
02.
03.
04.
05.#define setBit(val, bitn) (val |=(1<<(bitn)))
06.#define clearBit(val, bitn) (val&=~(1<<(bitn)))
07.#define getBit(val, bitn) (val &(1<<(bitn)) )
08.
09.int functionSpeed=0,functionFlag=0;
10.int PIDSign=true,SpeedPIDSign=true;;
11.int function3Sign=false;
12.
13./////////////Angle Parameter/////////
14.long setPoint=0,setPointOpp=0,originPoint=0;
15.int degree=0,degreeLast=0,setPointDegree=0;
16.
17.
18./////////Arm Speed Parameter/////////
19.#define PULSE_PER_ROUND 1600
20.#define MAX_SP_SPEED (PULSE_PER_ROUND*1)
21.#define MIN_SP_SPEED (-PULSE_PER_ROUND*1)
22.long setPosition=0,position=0,positionLast=0;
23.long absPositionLast=0,absPosition=0;
24.int setSpeed=0,armSpeed=0,lastSpeed=0;
25.int np=500,ni=0,nd=0;
26.long sp_error_p=0,sp_error_i=0,sp_error_d=0;
27.int tempSpSpeed=0,tempPosition;
28.int delta=2,maxDelta=8;
29.
30.int calcAngle(int x)
31.{
32. if(setPointOpp<=x&&x
33. return (x-setPointOpp)*-0.352;
34. else if(0<=x&&x
35. return (setPointOpp-x)*0.352;
36. else
37. return 180-(x-originPoint)*0.352;
38.
39.}
40.
41.
42.void setup()
43.{
44.
45. EncoderInit();
46. StepperInit();
47.
48. Serial.begin(57600);
49. Serial.print("test beginndelay 2sn");
50. int i,num=5,temp;
51. for(i=0;i
52. {
53. temp=SSI(0);
54. setPointOpp+=temp;//避免编码器0点在摆杆最低点或最高点,简化计算
55. Serial.println(temp);
56. delay(100);
57. }
58. Serial.println(setPointOpp);
59. setPointOpp/=num;
60. Serial.println(setPointOpp);
61. if(setPointOpp<512)
62. setPoint=setPointOpp+512;
63. else
64. setPoint=setPointOpp-512;
65. originPoint=setPoint;
66. setPointDegree=calcAngle(originPoint);
67. Serial.print("setPointOpp=");
68. Serial.println(setPointOpp);
69. Serial.print("setPoint=");
70. Serial.println(setPoint);
71. delay(2000);
72.73.
74. // for Timer2
75.
76. // interrupts every 1 ms
77.
78. TIMSK2 &= ~(1<
79. TCCR2A &= ~((1<
80. TCCR2B &= ~(1<
81. ASSR &= ~(1<
82. TIMSK2 &= ~(1<
83. TCCR2B |= (1<
84. TCCR2B &= ~(1<
85. TCNT2 = 131;
86. TIMSK2 |= (1<
87.
88. Timer1.attachInterrupt( StepperEvent ); // attach the service routine here
89.
90.}
91.
92.
93.int myspeed;
94.unsigned long timer;
95.
96.int angle,angleLast;
97.int error_p,error_i,error_d;
98.int velocity;
99.
100.int kp=15,kd=0,ki=20;//p=15,i=15 works!!
101.
102.unsigned int counter=0;
103.
104.
105.void loop()
106.{
107. checkParameter();
108. switch(functionFlag)
109. {
110. case 1:
111. function1();
112. break;
113. case 11:
114. function1();
115. break;
116. case 2:
117. function2();
118. break;
119. case 3:
120. function3();
121. break;
122. case 4:
123. function4();
124. break;
125. case 41:
126. function41();
127. break;
128. case 6:
129. function6();
130. break;
131.
132. }
133.
134.
135.
136. Serial.print(originPoint);
137. Serial.print(\',\');
138. Serial.print(setPoint);
139. Serial.print(\',\');
140. Serial.print(angle);
141. Serial.print(\',\');
142. Serial.print(degree);
143. Serial.print(\',\');
144. Serial.print(armSpeed);//sp_error_i
145. Serial.print(\',\');
146. Serial.print(absPosition);
147. Serial.print(\'n\');
148.
149. //delay(50);
150.}
151.
152.int calcSpeed(int pos,int posLast)
153.{
154. if(abs(pos-posLast<512))
155. return pos-posLast;
156. int temp;
157. if(pos>posLast)//counter clock
158. temp=1023-pos-posLast;
159. else if(pos
160. temp=pos+(1023-posLast);
161. if(temp>1000)
162. return temp-1023;
163. else if(temp<-1000)
164. return 1023+temp;
165. return temp;
166.}
167.
168.
169.int controlTimer=0,speedTimer=0;;
170.int tempPos,tempPosLast;
171.int armSpeedArr[3]={0};
172.ISR(TIMER2_OVF_vect) {
173. TIMSK2 |= (0<
174. TCNT2 = 131; // reload the timer
175. controlTimer++;
176. speedTimer++;
177. // estimate velocity
178. if (controlTimer == 5){
179. controlTimer = 0;
180. ////////////angle PID///////////
181. angleLast=angle;
182. angle=SSI(0);
183. degreeLast=degree;
184. degree=calcAngle(angle);
185. //angle=FIR2(angle);
186. velocity = (angle - angleLast);
187. if(abs(angle-setPoint)<=4)
188. error_p = 0;
189. else
190. error_p = setPoint-angle;
191. error_d = velocity;
192. error_i = error_i + error_p;
193.
194. if(PIDSign==false||abs(degree)<110)
195. {
196. error_p=0;
197. error_d=0;
198. error_i=0;
199. }
200. myspeed = - (kp*error_p + kd*error_d + ki*error_i/10);
201. setStepperSpeed(myspeed + tempSpSpeed+functionSpeed);
202. }
203.
204.
205. if(speedTimer==5)
206. {
207. speedTimer=0;
208. ////////Speed PID///////////////
209. positionLast=position;
210. lastSpeed=armSpeed;
211. position=SSI(1);
212.
213. armSpeed=calcSpeed(position,positionLast);
214. armSpeedArr[0]=armSpeedArr[1];
215. armSpeedArr[1]=armSpeedArr[2];
216. armSpeedArr[2]=armSpeed;
217. armSpeed=mid(armSpeedArr[0],armSpeedArr[1],armSpeedArr[2]);
218. if(abs(armSpeed)<=1)
219. armSpeed=0;
220.
221. absPositionLast=absPosition;
222. absPosition+=armSpeed;
223.
224. sp_error_p = setSpeed-armSpeed;
225. sp_error_d = armSpeed-lastSpeed;
226. sp_error_i = sp_error_i + sp_error_p;
227. sp_error_i=constrain(sp_error_i,-1000,1000);
228.
229.
230. tempSpSpeed = -(np*sp_error_p/10.0 + nd*sp_error_d/10.0 + ni*sp_error_i/100.0);
231. tempSpSpeed=constrain(tempSpSpeed,-delta,delta);
232. //if(PIDSign==false||SpeedPIDSign==false||abs(degree)>155)
233. if(PIDSign==true&&SpeedPIDSign==true&&abs(degree)>155)
234. {
235. setPoint+=tempSpSpeed;
236. if(setPoint>originPoint+maxDelta)
237. setPoint=originPoint+maxDelta;
238. else if(setPoint
239. setPoint=originPoint-maxDelta;
240. }
241. else if(SpeedPIDSign==true)
242. {
243. setPoint=originPoint;
244. }
245. }
246.
247. //setStepperSpeed(myspeed + tempSpSpeed);
248.
249. TIMSK2 |= (1<
250.}
附件代码资料下载如上:
|