Advertisement
Guest User

Untitled

a guest
Apr 19th, 2022
57
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 0.84 KB | None | 0 0
  1. uint32_t error[3];
  2. uint32_t A[3];
  3. uint32_t
  4. uint8_t Kp = 1;
  5. uint8_t Ki = 1;
  6. uint8_t Kd = 1;
  7. const
  8. A[0] = Kp + Ki*dt + Kd/dt
  9. A[1] = -Kp - 2*Kd/dt
  10. A[2] = Kd/dt
  11. error[2] := 0 // e(t-2)
  12. error[1] := 0 // e(t-1)
  13. error[0] := 0 // e(t)
  14. CT_UART.THR = 'S';
  15. CT_UART.THR = 'N';
  16. CT_UART.THR = '\r';
  17. reference_count = position // initil reading from decoder as unit32
  18. while (finished = false) {
  19. error[2] := error[1]
  20. error[1] := error[0]
  21. error[0] := (setpoint+reference_count) − measured_value // currentin reading from decoder
  22. output := output + A[0] * error[0] + A[1] * error[1] + A[2] * error[2]
  23. sleep(dt)
  24.  
  25. if (time +dt > period ){
  26. refrence_count = position // current reading from decoder.
  27. }
  28. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement