Diff: main.cpp

Revision:

8:895d941a5910

Parent:

7:f32005d13749

Child:

9:c722418997b5

--- a/main.cpp	Mon Oct 15 12:43:04 2018 +0000
+++ b/main.cpp	Mon Oct 15 14:03:05 2018 +0000
@@ -1,26 +1,83 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
+#include <math.h>
 
 DigitalOut directionpin1(D4);
 PwmOut pwmpin1(D5);
 AnalogIn potmetervalue1(A1);
 DigitalIn button2(D9);          //klopt dit?
-DigitalIn encoderA(D9);
-DigitalIn encoderB(D8);
+InterruptIn encoderA(D9);
+InterruptIn encoderB(D8);
 
 DigitalOut directionpin2(D7);
 PwmOut pwmpin2(D6);
 AnalogIn potmetervalue2(A2);
-DigitalIn button1(D10);          //klopt dit?
 
 MODSERIAL pc(USBTX, USBRX);
 
+int encoder = 0;
+   
+   
+void encoderA_rise()       
+{
+    if(encoderB==false)
+    {
+        encoder++;
+    }
+    else
+    {
+        encoder--;
+    }
+}
+
+void encoderA_fall()      
+{
+    if(encoderB==true)
+    {
+        encoder++;
+    }
+    else
+    {
+        encoder--;
+    }
+}
+
+void encoderB_rise()       
+{
+    if(encoderA==true)
+    {
+        encoder++;
+    }
+    else
+    {
+        encoder--;
+    }
+}
+
+void encoderB_fall()      
+{
+    if(encoderA==false)
+    {
+        encoder++;
+    }
+    else
+    {
+        encoder--;
+    }
+}
+
+
+
 int main()
 { 
     pc.baud(115200);
     pc.printf("hello\n\r");
     pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz 
 
+    encoderA.rise(&encoderA_rise);
+    encoderA.fall(&encoderA_fall);
+    encoderB.rise(&encoderB_rise);
+    encoderB.fall(&encoderB_fall);
     
     while (true)
     {
@@ -30,25 +87,15 @@
           
           float m1 = ((u1*2.0f)-1.0f);
           float m2 = ((u2*2.0f)-1.0f);
-          
-          int Ea = encoderA;
-          int Eb = encoderB;
-          
-            int static Eas = 0;
-            int static Ebs = 0;
-          
         
         pwmpin1 = fabs(m1*0.6f)+0.4f;     //pwm duty cycle can only be positive, floating, 0.4f is "inefficiënt", dit tellen we erbij op, en keer 0.6 om te corrigeren voor de helling.        
         directionpin1.write(m1>0);        //Indien waar, motor draait rechtsom. Indien niet waar, motor draait linksom. 
         wait(0.01f);                   //zodat de code niet oneindig doorgaat.
-        
         pwmpin2 = fabs(m2*0.6f)+0.4f;    
-        directionpin2.write(m2>0);     
+        directionpin2.write(m2>0);   
         
-        if(A==1 && A>B)
-        {  
-            
-            
+        double angle_encoder = encoder*2*pi/8400
+        pc.printf("Encoder count: %i\n\r",encoder); 
     }
 }