Tristan Vlogman / errorFetch

Updates error values

Dependents:   locomotion_pid_action_refactor_EMG

errorFetch.cpp@2:d22c458a8a78, 2017-10-22 (annotated)

Committer:
tvlogman
Date:
Sun Oct 22 07:56:59 2017 +0000
Revision:
2:d22c458a8a78
Parent:
1:9e2c9237d88b
Child:
3:71a7dd98fb2c
Changed errorFetch: now it takes a reference value as an argument instead of a refGen object. This to seperate concerns.

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 #include "refGen.h"
tvlogman 0:cb9eda46a58c 6
tvlogman 0:cb9eda46a58c 7 // Member function definitions
tvlogman 2:d22c458a8a78 8 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 9
tvlogman 0:cb9eda46a58c 10 }
tvlogman 0:cb9eda46a58c 11
tvlogman 2:d22c458a8a78 12 void errorFetch::fetchError(int counts, float ref){
tvlogman 1:9e2c9237d88b 13 // Calculating motor position based on encoder counts
tvlogman 0:cb9eda46a58c 14 double motorPosition = 2*3.14*(counts/(gearRatio*64.0f));
tvlogman 2:d22c458a8a78 15
tvlogman 0:cb9eda46a58c 16 // Computing position error
tvlogman 2:d22c458a8a78 17 e_pos = ref - motorPosition;
tvlogman 0:cb9eda46a58c 18
tvlogman 0:cb9eda46a58c 19 // Limiting the integral error to prevent integrator saturation
tvlogman 0:cb9eda46a58c 20 if(fabs(e_int) <= 5){
tvlogman 0:cb9eda46a58c 21 e_int = e_int + Ts*e_pos;
tvlogman 0:cb9eda46a58c 22 }
tvlogman 0:cb9eda46a58c 23
tvlogman 0:cb9eda46a58c 24 // Derivative error
tvlogman 0:cb9eda46a58c 25 e_der = (e_pos - e_prev)/Ts;
tvlogman 0:cb9eda46a58c 26 e_prev = e_pos; // Store current position error as we'll need it to compute the next derivative error
tvlogman 2:d22c458a8a78 27 }
tvlogman 2:d22c458a8a78 28