Diff: main.cpp

Revision:

32:1bb406d2b3c3

Parent:

31:cc08254ab7b5

Child:

33:6f4858b98fe5

--- a/main.cpp	Tue Oct 17 09:52:12 2017 +0000
+++ b/main.cpp	Fri Oct 20 12:24:37 2017 +0000
@@ -6,6 +6,7 @@
 #include "refGen.h"
 #include "controller.h"
 #include "motorConfig.h"
+#include "errorFetch.h"
 
 // Defining relevant constant parameters
 const float gearRatio = 131;
@@ -15,7 +16,8 @@
 const float k_i = 0; // Still needs a reasonable value
 const float k_d = 0; // Again, still need to pick a reasonable value
 
-QEI Encoder(D12,D13,NC,64, QEI::X4_ENCODING);
+QEI Encoder1(D12,D13,NC,64, QEI::X4_ENCODING);
+QEI Encoder2(D11,D10,NC,64, QEI::X4_ENCODING);
 MODSERIAL pc(USBTX, USBRX);
 HIDScope scope(5);
 
@@ -24,7 +26,9 @@
 InterruptIn sw3(SW3);
 InterruptIn button1(D2);
 InterruptIn button2(D3);
-AnalogIn pot2(A1);
+AnalogIn pot2(A3);
+AnalogIn emg0( A0 );
+//AnalogIn emg1( A1 );
 
 // PWM settings
 float pwmPeriod = 1.0/5000.0;
@@ -66,34 +70,12 @@
 const float Ts = 0.1;
 
 // Function getReferencePosition returns reference angle based on potmeter 1
-refGen ref(A0);
+refGen ref1(A1);
 
 //ref.getReferencePosition(maxAngle);
 
-// Initializing position and integral errors to zero
-float e_pos = 0;
-float e_int = 0;
-float e_der = 0;
-float e_prev = 0;
-
 // readEncoder reads counts and revs and logs results to serial window
-void getError(float &e_pos, float &e_int, float &e_der){
-    // Getting encoder counts and calculating motor position
-    counts = Encoder.getPulses();
-    double motor1Position = 2*3.14*(counts/(gearRatio*64.0f));
-
-    // Computing position error
-    e_pos = ref.getReferencePosition(maxAngle, r_direction) - motor1Position;
-    
-    // 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
-    }
+errorFetch err1(Encoder1, gearRatio, Ts, ref1);
 
 // Generate a PID controller with the specified values of k_p, k_d and k_i
 controller motorController1(k_p, k_d, k_i);
@@ -102,10 +84,10 @@
 motorConfig motor1(LED_GREEN,LED_RED,LED_BLUE,D5,D4);
     
 void measureAndControl(){
-    getError(e_pos, e_int, e_der);
-    float motorValue = motorController1.control(e_pos, e_int, e_der);
-    float r = ref.getReferencePosition(maxAngle, r_direction);
-    sendDataToPc(r, e_pos, e_int, e_der, motorValue);
+    err1.fetchError();
+    float motorValue = motorController1.control(err1.e_pos, err1.e_int, err1.e_der);
+    float r = ref1.getReferencePosition(ref.maxAngle, ref1.r_direction);
+    sendDataToPc(r, err1.e_pos, emg0.read(), err1.e_der, motorValue);
     pc.printf("Motorvalue is %.2f \r\n", motorValue);
     pc.printf("Error is %.2f \r\n", e_pos);
     pc.printf("Reference is %.2f \r\n", r);