Diff: main.cpp

Revision:

1:08e8cc33fcae

Parent:

0:e4858e2df9c7

Child:

2:d7286c36595f

diff -r e4858e2df9c7 -r 08e8cc33fcae main.cpp
--- a/main.cpp	Fri Oct 11 09:40:16 2019 +0000
+++ b/main.cpp	Fri Oct 11 09:53:22 2019 +0000
@@ -14,6 +14,10 @@
 DigitalIn encA(D13);
 DigitalIn encB(D12);
 QEI encoder(D13,D12,NC,64,QEI::X4_ENCODING);
+float T_encoder = 0.01;
+float angle;
+float omega;
+
 
 // Motor
 DigitalOut motor2Direction(D4);
@@ -24,9 +28,11 @@
 //Motorcontrol
 bool motordir;
 double motorpwm;
-double u;
+float u1;
+double u2;
 double potValue;
-
+double pi2= 6.283185;
+float fout;
 // PC connection
 MODSERIAL pc(USBTX, USBRX);
 
@@ -34,6 +40,7 @@
 Ticker motorTicker;
 Ticker controlTicker;
 Ticker directionTicker;
+Ticker encoderTicker;
 
 const float PWM_period = 1e-6;
 
@@ -49,10 +56,14 @@
 
 void motorControl()
 {
+    angle = counts * factorin / gearratio; // Angle of motor shaft in rad
+    omega = deltaCounts / T_encoder * factorin / gearratio; // Angular velocity of motor shaft in rad/s
     potValue= potmeter.read();
-    u= (potValue*2)-1;
-    motorpwm= abs(u);
-    if (u<0){
+    u1= (potValue*2*pi2)-pi2;
+    fout=u1-angle;
+    u2= fout/pi2;
+    motorpwm= abs(u2);
+    if (u2<0){
         motordir= 0;}
     else {
          motordir= 1;}
@@ -60,6 +71,13 @@
     motor1Direction= motordir;
 }
 
+void readEncoder()
+{
+    counts = encoder.getPulses();
+    deltaCounts = counts - countsPrev;
+
+    countsPrev = counts;
+}
 
 int main()
 {
@@ -69,10 +87,16 @@
     motor1Power.period(PWM_period);
     motorTicker.attach(motorControl, 0.01);
     
+    
+    encoderTicker.attach(readEncoder, T_encoder);
+   
     while (true) {
-        float potValue = potmeter.read(); // Read potmeter
 
-        pc.printf("Potmeter: %f \r\n", potValue);
+        pc.printf("Potmeter: %d \r\n", potValue);
+        pc.printf("Counts: %i   DeltaCounts: %i\r\n", counts, deltaCounts);
+        pc.printf("Angle:  %f   Omega:       %f\r\n", angle, omega);
+        pc.printf("U1: %f   Error:  %f     \r\n",u1, fout);
+        
         wait(0.5);
     }
 }