|
1. #include<reg52.h> 2. #include"lcd1602.h" 3. 4. sfr T2MOD =0x0c9; 5. #defineuchar unsigned char 6. #define uintunsigned int 7. 8. sbit Q0 =P2^4; 9. sbit Q1 =P2^5; 10. sbit Q2 =P2^6; 11. sbit Q3 =P2^7; 12. 13. sbitGORB = P1^6; //換相 14. sbit PWM =P1^7; 15. sbitUP = P1^0; 16. sbitDOWM = P1^1; 17. sbitADDSPEED = P1^2; 18. sbitSUBSPEED = P1^3; 19. 20. uint tuint =65535; 21. uint tpwm =1; //pwm周期為10000us tpwm變量表示pwm高電平時間，也相當于占空比 (仿真時，頻率高時，電機反應慢。在實物上要加大頻率) 22. uchart1_flag = 0; 23. 24. uint pulse =0; 25. uint t0_flag= 0; 26. uchart2_flag = 0; 27. bit t2_over= 0; 28. bit Just_Get= 1; 29. 30. 31. #define ZZ { Q0 = 0;Q1 = 0;Q2 = 1;Q3 = 1;} //正轉 32. #define FZ { Q0 = 1;Q1 = 1;Q2 = 0;Q3 = 0;} //反轉 33. #define STOP { Q0 = 1;Q1 = 0;Q2= 1;Q3 = 0;} //停止 34. //禁止出現 Q0 = 0;Q1 = 1;Q2 = 0;Q3 = 1; 不然會燒掉mos管 35. 36. //************************PID ************************************* 37. float now =0,bef = 0,bbef = 0; //本次采樣值，上次采樣值，上上次采樣值 38. floaterr_now,err_bef,err_bbef; //當前偏差，上次偏差，上上次偏差 39. floaterror_add = 0; //所有偏差之和 40. float set =25; //設定值 41. 42. float kp =25; 43. float ki =25; 44. float kd =0; 45. 46. //***************************************************************** 47. 48. voiddelayms(uint ms)//延時？個 ms 49. { 50. uchar a,b,c; 51. while(ms--) 52. { 53. for(c=1;c>0;c--) 54. for(b=142;b>0;b--) 55. for(a=2;a>0;a--); 56. } 57. } 58. 59. void timer_init() 60. { 61. EA = 1; 62. ET0 = 1; 63. ET1 = 1; 64. ET2 = 1; 65. 66. TMOD = 0x15; //定時器0 計數模式 定時器1模式1 67. T2MOD = 0x01; 68. 69. TH0 = TL0 = 255; 70. TH2 = 0x3C; 71. TL2 = 0xB0; //50MS 72. 73. } 74. voidtimer1() interrupt 3 75. { 76. if(t1_flag == 0) 77. { 78. t1_flag = 1; 79. PWM = 1; 80. TH1 = (tuint - tpwm + 1)/256; 81. TL1 = (tuint - tpwm + 1)%256; 82. 83. } 84. else 85. { 86. t1_flag = 0; 87. PWM = 0; 88. TH1 = (tuint - 10000 + tpwm +1)/256; 89. TL1 = (tuint - 10000 + tpwm +1)%256; 90. } 91. } 92. 93. voidtimer0() interrupt 1 94. { 95. TH0 = TL0 = 255; 96. t0_flag++; 97. } 98. voidtimer2() interrupt 5 99. { 100. TF2 = 0; 101. TH2 = 0x3C; 102. TL2 =0xB0; //50MS 103. 104. t2_flag++; 105. 106. if(t2_flag ==2) 107. { 108. TR0 = 0; 109. TR2 = 0; 110. t2_flag = 0; 111. t2_over = 1; //表示100ms時間到 112. } 113. } 114. void GetPulse() 115. { 116. t0_flag = 0; 117. t2_flag = 0; 118. 119. TH0 = TL0 =255; 120. TH2 = 0x3C; 121. TL2 =0xB0; //50MS 122. 123. TR0 = 1; 124. TR2 = 1; 125. } 126. 127. int PID() //增量式PID 128. { 129. int change; 130. 131. err_now = set -now; 132. err_bef = set -bef; 133. err_bbef = set- bbef; 134. 135. change =kp*(err_now - err_bef) + ki*err_now + kd*(err_now - 2*err_bef + err_bbef); 136. 137. /* 138. if(set >=now) 139. { 140. if(set - now > 1) 141. change = kp*(err_now - err_bef) +ki*err_now + kd*(err_now - 2*err_bef + err_bbef); 142. else 143. change = 0.2*kp*(err_now - err_bef) +0.5*ki*err_now + kd*(err_now - 2*err_bef + err_bbef); 144. } 145. else if(now> set) 146. { 147. if(now - set > 1) 148. change = kp*(err_now - err_bef) +ki*err_now + kd*(err_now - 2*err_bef + err_bbef); 149. else 150. change = 0.2*kp*(err_now - err_bef) +0.5*ki*err_now + kd*(err_now - 2*err_bef + err_bbef); 151. 152. } 153. */ 154. 155. //change = (kp+ ki + kd)*(set - now) + (-kp - 2*kd)*(set - bef) + kd*(set - bbef); 156. //change =kp*(set - now) + ki*(set - bef) + kd*(set - bbef); 157. if(change >0) 158. { 159. printchar(1,10,'+'); 160. printuint(1,11,4,change); 161. 162. } 163. else if(change< 0) 164. { 165. printchar(1,10,'-'); 166. printuint(1,11,4,-change); 167. } 168. else if(change== 0) 169. { 170. printchar(1,10,' '); 171. printword(1,11," 0 "); 172. 173. } 174. 175. return(change); 176. } 177. 178. int PID2() //位置式PID 179. { 180. 181. int num = 0; 182. static num_bef= 0; 183. 184. err_now = set -now; 185. err_bef = set -bef; 186. 187. error_add =error_add + err_now; //誤差累加。一旦誤差為0則error_add的值不變，PID輸出值不變 188. 189. num =kp*err_now + ki*error_add + kd*(err_now - err_bef); 190. 191. /* 192. if(set - now>= 0) 193. { 194. if(set - now > 1) 195. num = kp*err_now + ki*error_add +kd*(err_now - err_bef); 196. else 197. num = 0.1*kp*err_now + ki*error_add +kd*(err_now - err_bef); 198. } 199. else 200. { 201. if(now - set > 1) 202. num = kp*err_now + ki*error_add +kd*(err_now - err_bef); 203. else 204. num = 0.1*kp*err_now + ki*error_add +kd*(err_now - err_bef); 205. 206. } 207. */ 208. 209. if(num >num_bef) 210. { 211. printchar(1,10,'+'); 212. printuint(1,11,4,num - num_bef); 213. } 214. else if(num< num_bef) 215. { 216. printchar(1,10,'-'); 217. printuint(1,11,4,num_bef - num); 218. } 219. else 220. { 221. printchar(1,10,' '); 222. printuint(1,11,4,0); 223. } 224. 225. num_bef = num; 226. 227. return((uint)num); 228. } 229. 230. void main() 231. { 232. 233. lcd_init();//初始化LCD 234. timer_init();//初始化Timer 235. TH1 = TL1 =255; 236. 237. printword(0,0,"P:"); //比例系數 238. printword(0,5,"S:"); //設定值 239. printword(1,0,"TPWM:"); //當前占空比 240. printword(0,10,"PS:"); //當前電機反饋的每秒脈沖數 241. 242. while(1) 243. { 244. if(GORB == 1) 245. { ZZ; } 246. else 247. { FZ; } 248. 249. if(ADDSPEED == 0) 250. set++; 251. if(SUBSPEED == 0) 252. set--; 253. 254. if(Just_Get == 1) 255. { 256. Just_Get = 0; 257. GetPulse(); 258. } 259. else if(t2_over == 1) 260. { 261. t2_over = 0; 262. Just_Get = 1; 263. pulse = t0_flag; 264. bbef = bef; 265. bef = now; 266. now = t0_flag; 267. 268. if(set != 0) 269. { 270. TR1 = 1; 271. } 272. else 273. { 274. TR1 = 0; 275. PWM = 0; 276. } 277. 278. // tpwm = tpwm + PID(); //增量式PID 279. tpwm = PID2(); //位置式PID 280. 281. } 282. 283. if(UP == 0) 284. kp = kp + 1; 285. if(DOWM == 0) 286. kp = kp - 1; 287. 288. printuint(0,2,3,kp); 289. printuint(0,7,3,set); 290. printuint(1,5,4,tpwm); 291. printuint(0,13,5,pulse); 292. 293. } 294. 295. } 296.
| 
