Diff: main.cpp

Revision:

5:2ae500da8fe1

Parent:

3:4f281c336a8b

Child:

6:ea3b3f50c893

Child:

8:7dab565a208e

--- a/main.cpp	Mon Oct 07 13:02:09 2019 +0000
+++ b/main.cpp	Mon Oct 07 14:22:05 2019 +0000
@@ -1,6 +1,8 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
 #include "FastPWM.h"
+#include "QEI.h"
+#include <math.h>
 
 DigitalIn button1(D12);
 AnalogIn pot2(A0);
@@ -8,18 +10,41 @@
 DigitalOut motor1Direction(D7);
 FastPWM motor1Velocity(D6);
 Serial pc(USBTX, USBRX);
+QEI Encoder(D8,D9,NC,8400);
 volatile double frequency = 17000.0;
 volatile double period_signal = 1.0/frequency;
 float vel = 0.0f;
 float referencevelocity;
 float motorvalue2;
+double Kp = 17.5;
+int counts;
+float position1 = 0;
+float position2;
+float timeinterval = 0.001;
+float measuredvelocity;
+
+// README
+// positive referencevelocity corresponds to a counterclockwise motion
+
+
+//P control implementation (behaves like a spring)
+double P_controller(double error)
+{
+    //Proportional part:
+    double u_k = Kp * error;
+    
+    //sum all parts and return it
+    return u_k;
+}
 
 //get the measured velocity
 double getmeasuredvelocity()
 {   
-    float measuredvelocity;
-    measuredvelocity = pot2.read();
-    return measuredvelocity;
+    counts = Encoder.getPulses();
+    counts = counts % 8400;
+    position2 = counts/8400*2*3.141592;
+    measuredvelocity = (position2-position1)/timeinterval; 
+    position1 = position2;
 }
 
 //get the reference of the velocity: positive or negative
@@ -52,12 +77,16 @@
     pc.baud(115200);
     pc.printf("Starting...\r\n");
     motor1Velocity.period(period_signal);
-    john.attach(measureandcontrol, 0.001f);
+    john.attach(measureandcontrol, timeinterval);
     while(true)
     {
         wait(0.5);
-        pc.printf("velocity = %f\r\n", vel);
-        pc.printf("motor1Direction = %i\r\n", (int)motor1Direction);
-        pc.printf("motorvalue2 = %f\r\n", motorvalue2);
+        //pc.printf("velocity = %f\r\n", vel);
+        //pc.printf("motor1Direction = %i\r\n", (int)motor1Direction);
+        //pc.printf("motorvalue2 = %f\r\n", motorvalue2);
+        pc.printf("number of counts %i\r\n", counts);
+        pc.printf("measured velocity is %f\r\n", measuredvelocity);
+        pc.printf("position1 = %f\r\n",position1);
+        pc.printf("position2 = %f\r\n",position2);
     }
 }
\ No newline at end of file