Revision 2:46365fee3503, committed 2021-11-17

Comitter:

ea78anana

Date:

Wed Nov 17 04:49:59 2021 +0000

Parent:

1:4df9f94facd5

Commit message:

arduino version of encoder coding;

Changed in this revision

diff -r 4df9f94facd5 -r 46365fee3503 arduino2.cpp
--- a/arduino2.cpp	Tue Nov 02 15:27:52 2021 +0000
+++ b/arduino2.cpp	Wed Nov 17 04:49:59 2021 +0000
@@ -1,10 +1,11 @@
 #include "mbed.h"
 #include "arduino.h"
 
-#define PinA A1 //A phase
-#define PinZ A3 //Z phase
-#define PinB A2 //B phase
+#define PinA D9 //A phase
+#define PinZ D14     //Z phase
+#define PinB D8 //B phase
 
+#define time2 10000
 #define HIGH 1
 #define LOW 0
 
@@ -12,68 +13,133 @@
 unsigned long time1 = 0; // Time
 float time_cw;
 float time_ccw;
-int counter = 0;
-const float d = 0.06; //Radius of the wheel
+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;
 double time3;//Time of phase Z detected, use for calculate the velocity
 Timer f;
 
+
 DigitalIn a11(PinA);
 DigitalIn a12(PinB);
-InterruptIn a14(PinA);
+//InterruptIn a11(PinA);
 InterruptIn a13(PinZ);
+//InterruptIn a12(PinB);
 
 Serial pc(USBTX, USBRX);
 
-void Encode()
+void Encode0()
 {
 
     //Ignore noise pulses
     
-    //When the two reading is bigger than 5ms then count it as correct pulse
+    //When the two reading is bigger than 10ms then count it as correct pulse
     //uint32_t m = f.read_ms();
     
-    //if ((m - time1) > 5)
+    //if ((m - time1) > 0.2)
 
-    {
+    //{
 
-        //When Phase A is falling will be detected 
+        //When Phase A is falling will be detected
+        if((current == 1) && (a11 == HIGH) && (a12 == LOW))
         
-        if ((a11 == LOW) && (a12 == HIGH))
+        {   counter_cw++;
+            
+            pc.printf("Foward %d \r\n", counter_cw);
         
-        {   pc.printf("backward ");
-        
-            counter--;
         
         }
         
-             else
+             else if((current == 2) && (a11 == LOW) && (a12 == HIGH)) 
         
-        {    pc.printf("fordward ");
+        {  counter_ccw++; 
         
-              counter++;
+           pc.printf("Backward %d \r\n", counter_ccw);
+        
         
         }
     
-    }
+    //}
     
     //time1 = m;
     
 }
 
+void test(){
+    int n,m = 0;
+    n = a11;
+    m = a12;
+    pc.printf("%d %d \r\n", n, m);
+}
+
+void Encode1()
+{   //uint32_t m = f.read();
+    
+   //if ((m - time1) > 0.2)
+
+   //{
+    if(current == 0){
+      
+        if ((a11 == HIGH) && (a12 == LOW))
+        
+        {   pc.printf("Foward \r\n");
+        
+            current = 1;
+        
+        }
+        
+             else if ((a11 == LOW) && (a12 == HIGH)) 
+        
+        {    pc.printf("Backward \r\n");
+        
+              current = 2;
+        
+        }
+    
+    }else if ((current == 1) && (((a11 == HIGH) && (a12 == LOW)) || ((a11 == HIGH) && (a12 == HIGH))))
+        
+        {   counter_cw++;
+            
+            pc.printf("Foward %d \r\n", counter_cw);
+        
+        
+        }
+        
+             else if((current == 2) && (((a11 == LOW) && (a12 == HIGH)) || ((a11 == LOW) && (a12 == LOW)))) 
+        
+        {  counter_ccw++; 
+        
+           pc.printf("Backward %d \r\n", counter_ccw);
+        
+        
+        }
+    //}
+    //time1 = m;
+    //pc.printf("timer %d \r\n", time1);
+    
+}
+
 void setup(){
     a11.mode(PullUp);
     a12.mode(PullUp);
-    a13.fall(&Encode);
-    //a14.rise(&Encode);
-    //a14.fall(&Encode);
+    //a13.rise(&Encode1);
+    //pc.baud(57600);
+    a13.rise(&test);
+    //a13.fall(&Encode);
+    //a11.rise(&Encode1);
+    //a14.fall(&Encode1);
+    //a15.rise(&Encode);
+    //a15.fall(&Encode);
 }
 
 void Set_state(){
-    counter = 0;
+    counter_cw = 0;
+    counter_ccw = 0;
     uint32_t p = f.read_ms();
     time3 = p;//Time when Z phase detected
 }
@@ -84,32 +150,36 @@
     double  distance;
     //clockwise turning
     uint32_t n = f.read_ms();
-    if (counter == 25)
+    if (counter_cw == 20)
     {
 //      Printf("ok");//Testing
-      num = num + 25;
-      Set_state();
-      t = n- time3;
-      t = t / 1000;
-      distance = num * d * pi; 
-      velocity = d * pi / t;
-      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.");
-      pc.printf("The time is ");pc.printf("%f", t); pc.printf("s.");
-    }
-    //anti-clockwise turning
-    else if (counter == -25)
-    {
-//      Printf("ok");//Testing
-      num = num + 25;
+      num = num++;
       Set_state();
       t = n - time3;
       t = t / 1000;
       distance = num * d * pi; 
       velocity = d * pi / t;
-      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.");
-      pc.printf("The time is ");pc.printf("%f", t); pc.printf("s.");
+      current = 0;
+      pc.printf("turns: %d", 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.");
+      //pc.printf("The time is ");pc.printf("%f", t); pc.printf("s.");
+    }
+    //anti-clockwise turning
+    else if (counter_ccw == 20)
+    {
+//      Printf("ok");//Testing
+      num = num++;
+      Set_state();
+      t = n - time3;
+      t = t / 1000;
+      distance = num * d * pi; 
+      velocity = d * pi / t;
+      current = 0;
+      pc.printf("turns: %d", 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.");
+      //pc.printf("The time is ");pc.printf("%f", t); pc.printf("s.");
     }
     //pc.printf("%d \n",counter);
 }
@@ -121,6 +191,10 @@
     pc.printf("start");
     f.start();
     while(1){
-        loop();
+        uint32_t m = f.read();
+        if(m > 0.5){
+            current = 0;
+        }
+        //loop();
     }
 }
\ No newline at end of file