Diff: Encoder.cpp

Revision:

0:59348dbbd541

Child:

1:2c9a92a538e4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Encoder.cpp	Sat Nov 20 15:12:50 2021 +0000
@@ -0,0 +1,116 @@
+#include "mbed.h"
+
+#define PinA D9 //A phase
+#define PinZ D14     //Z phase
+#define PinB D8 //B phase
+
+#define time2 10000
+#define HIGH 1
+#define LOW 0
+
+//Initialize Variable
+unsigned long time1 = 0; // Time
+float time_cw;
+float time_ccw;
+int counter_cw = 0;
+int counter_ccw = 0;
+const float d = 0.058; //Diameter of the wheel
+const float pi = 3.141592654;//PI
+int num = 0;//number of turns
+double t;//time per turn
+float velocity;
+int current = 0;
+int temp = 0;
+int n = 0;
+double time3;//Time of phase Z detected, use for calculate the velocity
+Timer f;
+
+
+DigitalIn a12(PinB);
+InterruptIn a11(PinA);
+InterruptIn a13(PinZ);
+
+
+Serial pc(USBTX, USBRX);
+
+void Encode0()
+{
+        if((a11 == HIGH) && (a12 == LOW))
+        
+        {   counter_cw++;
+
+        }
+        
+             else
+        
+        {  counter_ccw++; 
+        
+        
+        
+        }
+    
+}
+
+void setup(){
+    a11.mode(PullUp);
+    a12.mode(PullUp);
+    a11.rise(&Encode0);
+}
+
+void Set_state(int a, int b){
+    counter_cw = a;
+    counter_ccw = b;
+    n = 0;
+}
+
+void loop()
+
+{
+    double  distance;
+    //clockwise turning
+    n = n + 2;
+    if (counter_cw >= 2500)
+    {
+//      Printf("ok");//Testing
+      temp = counter_cw / 2500;
+      num = num + temp;
+      current = counter_cw - 2500 * temp;
+      t = n;
+      Set_state(current, counter_ccw);
+      distance = num * d * pi; 
+      velocity = (temp * d * pi) / t;
+      current = 0;
+      pc.printf("turns: %d \r\n", num);
+      pc.printf("The wheel has run ");pc.printf("%f", distance); pc.printf("m.");
+      pc.printf("The cw_speed is ");pc.printf("%f", velocity); pc.printf("m/s.");
+    }
+    //anti-clockwise turning
+    else if (counter_ccw >= 2500)
+    {
+//      Printf("ok");//Testing
+      temp = counter_ccw / 2500;
+      num = num + temp;
+      current = counter_ccw - 2500 * temp;
+      t = n;
+      Set_state(counter_cw,current);
+      distance = num * d * pi; 
+      velocity = d * pi / t;
+      current = 0;
+      pc.printf("turns: %d \r\n", num);
+      pc.printf("The wheel has run ");pc.printf("%f", distance); pc.printf("m.");
+      pc.printf("The cw_speed is ");pc.printf("%f", velocity); pc.printf("m/s.");
+    }
+}
+
+
+
+int main(){
+    setup();
+    pc.printf("start");
+    f.start();
+    while(1){
+        loop();
+        wait(2);
+        pc.printf("%d %d \r\n", counter_cw, counter_ccw);
+    }
+}
\ No newline at end of file