Diff: errorFetch.cpp

Revision:

0:cb9eda46a58c

Child:

1:9e2c9237d88b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/errorFetch.cpp	Fri Oct 20 12:24:03 2017 +0000
@@ -0,0 +1,27 @@
+#include "errorFetch.h"
+#include "mbed.h"
+#include "QEI.h"
+#include "refGen.h"
+
+// Member function definitions
+errorFetch::errorFetch(QEI Encoder, float N, float TS, refGen REF):motorEncoder(Encoder), gearRatio(N), Ts(TS), e_pos(0), e_int(0), e_der(0), e_prev(0), ref(REF){
+
+    }
+    
+void errorFetch::getError(){
+    // Getting encoder counts and calculating motor position
+    int counts = motorEncoder.getPulses();
+    double motorPosition = 2*3.14*(counts/(gearRatio*64.0f));
+
+    // Computing position error
+    e_pos = ref.getReferencePosition(ref.maxAngle, ref.r_direction) - 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
+    }
\ No newline at end of file