Tristan Vlogman / errorFetch

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?

UserRevisionLine numberNew 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