1
0

pid_controller.ino 7.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296
  1. /************************************************************************
  2. Description:PID控制器的代码实现。
  3. Author: www.corvin.cn
  4. History: 20180209: init this file;
  5. **************************************************************************/
  6. #include "pid_controller.h"
  7. #include "encoder_driver.h"
  8. static SetPointInfo AWheelPID, BWheelPID, CWheelPID;
  9. /*default PID Parameters */
  10. static int AWheel_Kp = 11;
  11. static int AWheel_Kd = 15;
  12. static int AWheel_Ki = 0;
  13. static int AWheel_Ko = 50;
  14. static int BWheel_Kp = 11;
  15. static int BWheel_Kd = 15;
  16. static int BWheel_Ki = 0;
  17. static int BWheel_Ko = 50;
  18. static int CWheel_Kp = 11;
  19. static int CWheel_Kd = 15;
  20. static int CWheel_Ki = 0;
  21. static int CWheel_Ko = 50;
  22. static unsigned char moving = 0; // is the base in motion?
  23. void setWheelPIDTarget(int value_A, int value_B, int value_C)
  24. {
  25. AWheelPID.TargetTicksPerFrame = value_A;
  26. BWheelPID.TargetTicksPerFrame = value_B;
  27. CWheelPID.TargetTicksPerFrame = value_C;
  28. }
  29. unsigned char getMoveStatus(void)
  30. {
  31. return moving;
  32. }
  33. void setMoveStatus(unsigned char state)
  34. {
  35. moving = state;
  36. }
  37. void updatePIDParam(int index, int kp, int kd, int ki, int ko)
  38. {
  39. switch (index)
  40. {
  41. case A_WHEEL:
  42. AWheel_Kp = kp;
  43. AWheel_Kd = kd;
  44. AWheel_Ki = ki;
  45. AWheel_Ko = ko;
  46. break;
  47. case B_WHEEL:
  48. BWheel_Kp = kp;
  49. BWheel_Kd = kd;
  50. BWheel_Ki = ki;
  51. BWheel_Ko = ko;
  52. break;
  53. case C_WHEEL:
  54. CWheel_Kp = kp;
  55. CWheel_Kd = kd;
  56. CWheel_Ki = ki;
  57. CWheel_Ko = ko;
  58. break;
  59. default:
  60. break;
  61. }
  62. }
  63. /*
  64. Initialize PID variables to zero to prevent startup spikes
  65. when turning PID on to start moving
  66. In particular, assign both Encoder and PrevEnc the current encoder value
  67. See http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
  68. Note that the assumption here is that PID is only turned on
  69. when going from stop to moving, that's why we can init everything on zero.
  70. */
  71. void resetPID(void)
  72. {
  73. AWheelPID.TargetTicksPerFrame = 0.0;
  74. AWheelPID.Encoder = readEncoder(A_WHEEL);
  75. AWheelPID.PrevEnc = AWheelPID.Encoder;
  76. AWheelPID.output = 0L;
  77. AWheelPID.PrevInput = 0;
  78. AWheelPID.ITerm = 0;
  79. BWheelPID.TargetTicksPerFrame = 0.0;
  80. BWheelPID.Encoder = readEncoder(B_WHEEL);
  81. BWheelPID.PrevEnc = BWheelPID.Encoder;
  82. BWheelPID.output = 0L;
  83. BWheelPID.PrevInput = 0;
  84. BWheelPID.ITerm = 0;
  85. CWheelPID.TargetTicksPerFrame = 0.0;
  86. CWheelPID.Encoder = readEncoder(C_WHEEL);
  87. CWheelPID.PrevEnc = CWheelPID.Encoder;
  88. CWheelPID.output = 0L;
  89. CWheelPID.PrevInput = 0;
  90. CWheelPID.ITerm = 0;
  91. }
  92. /* PID routine to compute the next A motor commands */
  93. void doAWheelPID(SetPointInfo *p)
  94. {
  95. long Perror = 0L;
  96. long output = 0L;
  97. int input = 0;
  98. p->Encoder = readEncoder(A_WHEEL);
  99. input = p->Encoder - p->PrevEnc;
  100. Perror = p->TargetTicksPerFrame - input;
  101. /*
  102. Avoid derivative kick and allow tuning changes,
  103. see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
  104. see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
  105. */
  106. output = (AWheel_Kp * Perror - AWheel_Kd * (input - p->PrevInput) + p->ITerm) / AWheel_Ko;
  107. p->PrevEnc = p->Encoder; //save current encoder value to preEncoder
  108. // Accumulate Integral error *or* Limit output. Stop accumulating when output saturates
  109. output += p->output;
  110. if (output >= MAX_PWM)
  111. {
  112. output = MAX_PWM;
  113. }
  114. else if (output <= -MAX_PWM)
  115. {
  116. output = -MAX_PWM;
  117. }
  118. else
  119. {
  120. /*
  121. allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
  122. */
  123. p->ITerm += AWheel_Ki * Perror;
  124. }
  125. p->output = output; //save current pid output for next pid
  126. p->PrevInput = input;
  127. }
  128. /* PID routine to compute the next motor commands */
  129. void doBWheelPID(SetPointInfo *p)
  130. {
  131. long Perror = 0L;
  132. long output = 0L;
  133. int input = 0;
  134. p->Encoder = readEncoder(B_WHEEL);
  135. input = p->Encoder - p->PrevEnc;
  136. Perror = p->TargetTicksPerFrame - input;
  137. /*
  138. Avoid derivative kick and allow tuning changes,
  139. see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
  140. see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
  141. */
  142. output = (BWheel_Kp * Perror - BWheel_Kd * (input - p->PrevInput) + p->ITerm) / BWheel_Ko;
  143. p->PrevEnc = p->Encoder;
  144. // Accumulate Integral error *or* Limit output. Stop accumulating when output saturates
  145. output += p->output;
  146. if (output >= MAX_PWM)
  147. {
  148. output = MAX_PWM;
  149. }
  150. else if (output <= -MAX_PWM)
  151. {
  152. output = -MAX_PWM;
  153. }
  154. else
  155. {
  156. /*
  157. allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
  158. */
  159. p->ITerm += BWheel_Ki * Perror;
  160. }
  161. p->output = output;
  162. p->PrevInput = input;
  163. }
  164. /* PID routine to compute the next motor commands */
  165. void doCWheelPID(SetPointInfo *p)
  166. {
  167. long Perror = 0L;
  168. long output = 0L;
  169. int input = 0;
  170. p->Encoder = readEncoder(C_WHEEL);
  171. input = p->Encoder - p->PrevEnc;
  172. Perror = p->TargetTicksPerFrame - input;
  173. /*
  174. Avoid derivative kick and allow tuning changes,
  175. see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
  176. see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
  177. */
  178. output = (CWheel_Kp * Perror - CWheel_Kd * (input - p->PrevInput) + p->ITerm) / CWheel_Ko;
  179. p->PrevEnc = p->Encoder;
  180. // Accumulate Integral error *or* Limit output. Stop accumulating when output saturates
  181. output += p->output;
  182. if (output >= MAX_PWM)
  183. {
  184. output = MAX_PWM;
  185. }
  186. else if (output <= -MAX_PWM)
  187. {
  188. output = -MAX_PWM;
  189. }
  190. else
  191. {
  192. /*
  193. allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
  194. */
  195. p->ITerm += CWheel_Ki * Perror;
  196. }
  197. p->output = output;
  198. p->PrevInput = input;
  199. }
  200. /* Read the encoder values and call the PID routine */
  201. void updatePID(void)
  202. {
  203. if (!moving) /* If we're not moving, resetPID() there is nothing more to do */
  204. {
  205. /*
  206. Reset PIDs once, to prevent startup spikes,
  207. see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
  208. PrevInput is considered a good proxy to detect whether reset has already happened
  209. */
  210. if (AWheelPID.PrevInput != 0 || BWheelPID.PrevInput != 0 || CWheelPID.PrevInput != 0)
  211. {
  212. resetPID();
  213. }
  214. }
  215. else
  216. {
  217. /* Compute PID update for each motor */
  218. doAWheelPID(&AWheelPID);
  219. doBWheelPID(&BWheelPID);
  220. doCWheelPID(&CWheelPID);
  221. /* Set the motor speeds accordingly */
  222. setMotorSpeeds(AWheelPID.output, BWheelPID.output, CWheelPID.output);
  223. }
  224. }
  225. long readPidIn(int wheel)
  226. {
  227. long pidin = 0L;
  228. if (wheel == A_WHEEL)
  229. {
  230. pidin = AWheelPID.PrevInput;
  231. }
  232. else if (wheel == B_WHEEL)
  233. {
  234. pidin = BWheelPID.PrevInput;
  235. }
  236. else
  237. {
  238. pidin = CWheelPID.PrevInput;
  239. }
  240. return pidin;
  241. }
  242. long readPidOut(int wheel)
  243. {
  244. long pidout = 0L;
  245. if (wheel == A_WHEEL)
  246. {
  247. pidout = AWheelPID.output;
  248. }
  249. else if (wheel == B_WHEEL)
  250. {
  251. pidout = BWheelPID.output;
  252. }
  253. else
  254. {
  255. pidout = CWheelPID.output;
  256. }
  257. return pidout;
  258. }