Updates error values
Dependents: locomotion_pid_action_refactor_EMG
errorFetch.cpp@3:71a7dd98fb2c, 2017-10-22 (annotated)
- Committer:
- tvlogman
- Date:
- Sun Oct 22 08:01:40 2017 +0000
- Revision:
- 3:71a7dd98fb2c
- Parent:
- 2:d22c458a8a78
- Child:
- 4:2af658a4e54b
Changed so errorFetch no longer requires a refGen object but instead takes a reference value as a parameter - clarity and seperation of concerns;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tvlogman | 0:cb9eda46a58c | 1 | #include "errorFetch.h" |
tvlogman | 1:9e2c9237d88b | 2 | |
tvlogman | 0:cb9eda46a58c | 3 | #include "mbed.h" |
tvlogman | 0:cb9eda46a58c | 4 | #include "QEI.h" |
tvlogman | 0:cb9eda46a58c | 5 | |
tvlogman | 0:cb9eda46a58c | 6 | // Member function definitions |
tvlogman | 2:d22c458a8a78 | 7 | errorFetch::errorFetch(float N, float Ts):gearRatio(N), Ts(Ts), e_pos(0), e_int(0), e_der(0), e_prev(0){ |
tvlogman | 0:cb9eda46a58c | 8 | |
tvlogman | 0:cb9eda46a58c | 9 | } |
tvlogman | 0:cb9eda46a58c | 10 | |
tvlogman | 2:d22c458a8a78 | 11 | void errorFetch::fetchError(int counts, float ref){ |
tvlogman | 1:9e2c9237d88b | 12 | // Calculating motor position based on encoder counts |
tvlogman | 0:cb9eda46a58c | 13 | double motorPosition = 2*3.14*(counts/(gearRatio*64.0f)); |
tvlogman | 2:d22c458a8a78 | 14 | |
tvlogman | 0:cb9eda46a58c | 15 | // Computing position error |
tvlogman | 2:d22c458a8a78 | 16 | e_pos = ref - motorPosition; |
tvlogman | 0:cb9eda46a58c | 17 | |
tvlogman | 0:cb9eda46a58c | 18 | // Limiting the integral error to prevent integrator saturation |
tvlogman | 0:cb9eda46a58c | 19 | if(fabs(e_int) <= 5){ |
tvlogman | 0:cb9eda46a58c | 20 | e_int = e_int + Ts*e_pos; |
tvlogman | 0:cb9eda46a58c | 21 | } |
tvlogman | 0:cb9eda46a58c | 22 | |
tvlogman | 0:cb9eda46a58c | 23 | // Derivative error |
tvlogman | 0:cb9eda46a58c | 24 | e_der = (e_pos - e_prev)/Ts; |
tvlogman | 0:cb9eda46a58c | 25 | e_prev = e_pos; // Store current position error as we'll need it to compute the next derivative error |
tvlogman | 2:d22c458a8a78 | 26 | } |
tvlogman | 2:d22c458a8a78 | 27 |