Updates error values
Dependents: locomotion_pid_action_refactor_EMG
errorFetch.cpp
- Committer:
- tvlogman
- Date:
- 2017-10-26
- Revision:
- 4:2af658a4e54b
- Parent:
- 3:71a7dd98fb2c
File content as of revision 4:2af658a4e54b:
#include "errorFetch.h" #include "mbed.h" #include "QEI.h" // Member function definitions errorFetch::errorFetch(float N, float Ts):gearRatio(N), Ts(Ts), e_pos(0), e_int(0), e_der(0), e_prev(0){ } double errorFetch::fetchMotorPosition(int counts){ double motorPosition = 2*3.14*(counts/(gearRatio*64.0f)); return motorPosition; } void errorFetch::fetchError(double motorPosition, double ref){ // Computing position error e_pos = ref - motorPosition; // Limiting the integral error to prevent integrator saturation if(fabs(e_int) <= 5){ e_int = e_int + Ts*e_pos; } // Derivative error e_der = (e_pos - e_prev)/Ts; e_prev = e_pos; // Store current position error as we'll need it to compute the next derivative error }