Diff: main.cpp

Revision:

33:6f4858b98fe5

Parent:

32:1bb406d2b3c3

Child:

34:1a70aa045c8f

--- a/main.cpp	Fri Oct 20 12:24:37 2017 +0000
+++ b/main.cpp	Fri Oct 20 16:30:26 2017 +0000
@@ -1,3 +1,4 @@
+#include <vector>
 #include "mbed.h"
 #include "MODSERIAL.h"
 #include "HIDScope.h"
@@ -8,14 +9,30 @@
 #include "motorConfig.h"
 #include "errorFetch.h"
 
-// Defining relevant constant parameters
-const float gearRatio = 131;
+// ADJUSTABLE PARAMETERS
 
 // Controller parameters
 const float k_p = 1;
 const float k_i = 0; // Still needs a reasonable value
 const float k_d = 0; // Again, still need to pick a reasonable value
 
+// Defining motor gear ratio - for BOTH motors as this is the same in the current configuration
+const float gearRatio = 131;
+
+
+
+// Declaring a controller ticker and volatile variables to store encoder counts and revs
+Ticker controllerTicker;
+volatile int m1counts = 0;
+volatile int m2counts = 0;
+volatile float m1revs = 0.00;
+volatile float m2revs = 0.00;
+
+// PWM settings
+float pwmPeriod = 1.0/5000.0;
+int frequency_pwm = 10000; //10kHz PWM
+
+// Initializing encoder
 QEI Encoder1(D12,D13,NC,64, QEI::X4_ENCODING);
 QEI Encoder2(D11,D10,NC,64, QEI::X4_ENCODING);
 MODSERIAL pc(USBTX, USBRX);
@@ -30,11 +47,6 @@
 AnalogIn emg0( A0 );
 //AnalogIn emg1( A1 );
 
-// PWM settings
-float pwmPeriod = 1.0/5000.0;
-int frequency_pwm = 10000; //10kHz PWM
-
-
 // Setting up HIDscope
 volatile float x;
 volatile float y;  
@@ -57,25 +69,19 @@
         scope.send(); // send what's in scope memory to PC
 }
 
-// Initializing encoder
-Ticker encoderTicker;
-Ticker controllerTicker;
-volatile int counts = 0;
-volatile float revs = 0.00;
 
-// MOTOR CONTROL PART    
-bool r_direction = false;
+// REFERENCE PARAMETERS
 int posRevRange = 2; // describes the ends of the position range in complete motor output shaft revolutions in both directions
 const float maxAngle = 2*3.14*posRevRange; // max angle in radians
 const float Ts = 0.1;
 
 // Function getReferencePosition returns reference angle based on potmeter 1
-refGen ref1(A1);
-
-//ref.getReferencePosition(maxAngle);
+refGen ref1(A1, maxAngle);
+refGen ref2(A1, maxAngle);
 
 // readEncoder reads counts and revs and logs results to serial window
-errorFetch err1(Encoder1, gearRatio, Ts, ref1);
+errorFetch e1(ref1, gearRatio, Ts);
+errorFetch e2(ref2, gearRatio, Ts);
 
 // Generate a PID controller with the specified values of k_p, k_d and k_i
 controller motorController1(k_p, k_d, k_i);
@@ -84,19 +90,17 @@
 motorConfig motor1(LED_GREEN,LED_RED,LED_BLUE,D5,D4);
     
 void measureAndControl(){
-    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);
-    //pc.printf("motor1 direction is %d \r\n", motor1_direction.read());
+    m1counts = Encoder1.getPulses();
+    m2counts = Encoder2.getPulses();
+    e1.fetchError(m1counts);
+    float motorValue = motorController1.control(e1.e_pos, e1.e_int, e1.e_der);
+    float r = ref1.getReferencePosition();
+    sendDataToPc(r, e1.e_pos, emg0.read(), e1.e_der, motorValue);
     motor1.setMotor(motorValue);
     }
 
 void rSwitchDirection(){
-    r_direction = !r_direction;
+    ref1.r_direction = !ref1.r_direction;
     pc.printf("Switched reference direction! \r\n");
     }
 
@@ -109,7 +113,6 @@
     button2.rise(&rSwitchDirection);
     pc.baud(115200);
     controllerTicker.attach(measureAndControl, Ts);
-      
     pc.printf("Encoder ticker attached and baudrate set");    
     }