Sik Chiu Chow / Mbed 2 deprecated Encoder_Final_Version

Arduino code converted to Mbed

Dependencies:   mbed

Encoder.cpp@0:59348dbbd541, 2021-11-20 (annotated)

Committer:
ea78anana
Date:
Sat Nov 20 15:12:50 2021 +0000
Revision:
0:59348dbbd541
Child:
1:2c9a92a538e4
Final version of the encoder

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ea78anana 0:59348dbbd541 1 #include "mbed.h"
ea78anana 0:59348dbbd541 2
ea78anana 0:59348dbbd541 3 #define PinA D9 //A phase
ea78anana 0:59348dbbd541 4 #define PinZ D14 //Z phase
ea78anana 0:59348dbbd541 5 #define PinB D8 //B phase
ea78anana 0:59348dbbd541 6
ea78anana 0:59348dbbd541 7 #define time2 10000
ea78anana 0:59348dbbd541 8 #define HIGH 1
ea78anana 0:59348dbbd541 9 #define LOW 0
ea78anana 0:59348dbbd541 10
ea78anana 0:59348dbbd541 11 //Initialize Variable
ea78anana 0:59348dbbd541 12 unsigned long time1 = 0; // Time
ea78anana 0:59348dbbd541 13 float time_cw;
ea78anana 0:59348dbbd541 14 float time_ccw;
ea78anana 0:59348dbbd541 15 int counter_cw = 0;
ea78anana 0:59348dbbd541 16 int counter_ccw = 0;
ea78anana 0:59348dbbd541 17 const float d = 0.058; //Diameter of the wheel
ea78anana 0:59348dbbd541 18 const float pi = 3.141592654;//PI
ea78anana 0:59348dbbd541 19 int num = 0;//number of turns
ea78anana 0:59348dbbd541 20 double t;//time per turn
ea78anana 0:59348dbbd541 21 float velocity;
ea78anana 0:59348dbbd541 22 int current = 0;
ea78anana 0:59348dbbd541 23 int temp = 0;
ea78anana 0:59348dbbd541 24 int n = 0;
ea78anana 0:59348dbbd541 25 double time3;//Time of phase Z detected, use for calculate the velocity
ea78anana 0:59348dbbd541 26 Timer f;
ea78anana 0:59348dbbd541 27
ea78anana 0:59348dbbd541 28
ea78anana 0:59348dbbd541 29 DigitalIn a12(PinB);
ea78anana 0:59348dbbd541 30 InterruptIn a11(PinA);
ea78anana 0:59348dbbd541 31 InterruptIn a13(PinZ);
ea78anana 0:59348dbbd541 32
ea78anana 0:59348dbbd541 33
ea78anana 0:59348dbbd541 34 Serial pc(USBTX, USBRX);
ea78anana 0:59348dbbd541 35
ea78anana 0:59348dbbd541 36 void Encode0()
ea78anana 0:59348dbbd541 37 {
ea78anana 0:59348dbbd541 38 if((a11 == HIGH) && (a12 == LOW))
ea78anana 0:59348dbbd541 39
ea78anana 0:59348dbbd541 40 { counter_cw++;
ea78anana 0:59348dbbd541 41
ea78anana 0:59348dbbd541 42 }
ea78anana 0:59348dbbd541 43
ea78anana 0:59348dbbd541 44 else
ea78anana 0:59348dbbd541 45
ea78anana 0:59348dbbd541 46 { counter_ccw++;
ea78anana 0:59348dbbd541 47
ea78anana 0:59348dbbd541 48
ea78anana 0:59348dbbd541 49
ea78anana 0:59348dbbd541 50 }
ea78anana 0:59348dbbd541 51
ea78anana 0:59348dbbd541 52 }
ea78anana 0:59348dbbd541 53
ea78anana 0:59348dbbd541 54 void setup(){
ea78anana 0:59348dbbd541 55 a11.mode(PullUp);
ea78anana 0:59348dbbd541 56 a12.mode(PullUp);
ea78anana 0:59348dbbd541 57 a11.rise(&Encode0);
ea78anana 0:59348dbbd541 58 }
ea78anana 0:59348dbbd541 59
ea78anana 0:59348dbbd541 60 void Set_state(int a, int b){
ea78anana 0:59348dbbd541 61 counter_cw = a;
ea78anana 0:59348dbbd541 62 counter_ccw = b;
ea78anana 0:59348dbbd541 63 n = 0;
ea78anana 0:59348dbbd541 64 }
ea78anana 0:59348dbbd541 65
ea78anana 0:59348dbbd541 66 void loop()
ea78anana 0:59348dbbd541 67
ea78anana 0:59348dbbd541 68 {
ea78anana 0:59348dbbd541 69 double distance;
ea78anana 0:59348dbbd541 70 //clockwise turning
ea78anana 0:59348dbbd541 71 n = n + 2;
ea78anana 0:59348dbbd541 72 if (counter_cw >= 2500)
ea78anana 0:59348dbbd541 73 {
ea78anana 0:59348dbbd541 74 // Printf("ok");//Testing
ea78anana 0:59348dbbd541 75 temp = counter_cw / 2500;
ea78anana 0:59348dbbd541 76 num = num + temp;
ea78anana 0:59348dbbd541 77 current = counter_cw - 2500 * temp;
ea78anana 0:59348dbbd541 78 t = n;
ea78anana 0:59348dbbd541 79 Set_state(current, counter_ccw);
ea78anana 0:59348dbbd541 80 distance = num * d * pi;
ea78anana 0:59348dbbd541 81 velocity = (temp * d * pi) / t;
ea78anana 0:59348dbbd541 82 current = 0;
ea78anana 0:59348dbbd541 83 pc.printf("turns: %d \r\n", num);
ea78anana 0:59348dbbd541 84 pc.printf("The wheel has run ");pc.printf("%f", distance); pc.printf("m.");
ea78anana 0:59348dbbd541 85 pc.printf("The cw_speed is ");pc.printf("%f", velocity); pc.printf("m/s.");
ea78anana 0:59348dbbd541 86 }
ea78anana 0:59348dbbd541 87 //anti-clockwise turning
ea78anana 0:59348dbbd541 88 else if (counter_ccw >= 2500)
ea78anana 0:59348dbbd541 89 {
ea78anana 0:59348dbbd541 90 // Printf("ok");//Testing
ea78anana 0:59348dbbd541 91 temp = counter_ccw / 2500;
ea78anana 0:59348dbbd541 92 num = num + temp;
ea78anana 0:59348dbbd541 93 current = counter_ccw - 2500 * temp;
ea78anana 0:59348dbbd541 94 t = n;
ea78anana 0:59348dbbd541 95 Set_state(counter_cw,current);
ea78anana 0:59348dbbd541 96 distance = num * d * pi;
ea78anana 0:59348dbbd541 97 velocity = d * pi / t;
ea78anana 0:59348dbbd541 98 current = 0;
ea78anana 0:59348dbbd541 99 pc.printf("turns: %d \r\n", num);
ea78anana 0:59348dbbd541 100 pc.printf("The wheel has run ");pc.printf("%f", distance); pc.printf("m.");
ea78anana 0:59348dbbd541 101 pc.printf("The cw_speed is ");pc.printf("%f", velocity); pc.printf("m/s.");
ea78anana 0:59348dbbd541 102 }
ea78anana 0:59348dbbd541 103 }
ea78anana 0:59348dbbd541 104
ea78anana 0:59348dbbd541 105
ea78anana 0:59348dbbd541 106
ea78anana 0:59348dbbd541 107 int main(){
ea78anana 0:59348dbbd541 108 setup();
ea78anana 0:59348dbbd541 109 pc.printf("start");
ea78anana 0:59348dbbd541 110 f.start();
ea78anana 0:59348dbbd541 111 while(1){
ea78anana 0:59348dbbd541 112 loop();
ea78anana 0:59348dbbd541 113 wait(2);
ea78anana 0:59348dbbd541 114 pc.printf("%d %d \r\n", counter_cw, counter_ccw);
ea78anana 0:59348dbbd541 115 }
ea78anana 0:59348dbbd541 116 }