Updates error values
Dependents: locomotion_pid_action_refactor_EMG
errorFetch.cpp@4:2af658a4e54b, 2017-10-26 (annotated)
- Committer:
- tvlogman
- Date:
- Thu Oct 26 14:02:01 2017 +0000
- Revision:
- 4:2af658a4e54b
- Parent:
- 3:71a7dd98fb2c
changed so it has a seperate fetchMotorPosition function
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 | 4:2af658a4e54b | 10 | |
tvlogman | 4:2af658a4e54b | 11 | double errorFetch::fetchMotorPosition(int counts){ |
tvlogman | 4:2af658a4e54b | 12 | double motorPosition = 2*3.14*(counts/(gearRatio*64.0f)); |
tvlogman | 4:2af658a4e54b | 13 | return motorPosition; |
tvlogman | 4:2af658a4e54b | 14 | } |
tvlogman | 0:cb9eda46a58c | 15 | |
tvlogman | 4:2af658a4e54b | 16 | void errorFetch::fetchError(double motorPosition, double ref){ |
tvlogman | 0:cb9eda46a58c | 17 | // Computing position error |
tvlogman | 2:d22c458a8a78 | 18 | e_pos = ref - motorPosition; |
tvlogman | 0:cb9eda46a58c | 19 | |
tvlogman | 0:cb9eda46a58c | 20 | // Limiting the integral error to prevent integrator saturation |
tvlogman | 0:cb9eda46a58c | 21 | if(fabs(e_int) <= 5){ |
tvlogman | 0:cb9eda46a58c | 22 | e_int = e_int + Ts*e_pos; |
tvlogman | 0:cb9eda46a58c | 23 | } |
tvlogman | 0:cb9eda46a58c | 24 | |
tvlogman | 0:cb9eda46a58c | 25 | // Derivative error |
tvlogman | 0:cb9eda46a58c | 26 | e_der = (e_pos - e_prev)/Ts; |
tvlogman | 0:cb9eda46a58c | 27 | e_prev = e_pos; // Store current position error as we'll need it to compute the next derivative error |
tvlogman | 2:d22c458a8a78 | 28 | } |
tvlogman | 2:d22c458a8a78 | 29 |