Arduino code converted to Mbed
Embed:
(wiki syntax)
Show/hide line numbers
Encoder.cpp
00001 #include "mbed.h" 00002 00003 #define PinA D9 //A phase 00004 #define PinZ D14 //Z phase 00005 #define PinB D8 //B phase 00006 00007 #define time2 10000 00008 #define HIGH 1 00009 #define LOW 0 00010 00011 //Initialize Variable 00012 int counter_cw = 0; 00013 const float d = 0.058; //Diameter of the wheel 00014 const float pi = 3.141592654;//PI 00015 int num = 0;//number of turns 00016 double t;//time per turn 00017 float velocity; 00018 int current = 0; 00019 int temp = 0; 00020 int n = 0; 00021 double time3;//Time of phase Z detected, use for calculate the velocity 00022 Timer f; 00023 00024 00025 DigitalIn a12(PinB); 00026 InterruptIn a11(PinA); 00027 InterruptIn a13(PinZ); 00028 00029 00030 Serial pc(USBTX, USBRX); 00031 00032 void Encode0() 00033 { 00034 if((a11 == HIGH) && (a12 == LOW)) 00035 00036 { counter_cw++; 00037 00038 } 00039 00040 else 00041 00042 { counter_cw--; 00043 00044 00045 00046 } 00047 00048 } 00049 00050 void setup(){ 00051 a11.mode(PullUp); 00052 a12.mode(PullUp); 00053 a11.rise(&Encode0); 00054 } 00055 00056 void Set_state(int a){ 00057 counter_cw = a; 00058 n = 0; 00059 } 00060 00061 void loop() 00062 00063 { 00064 double distance; 00065 //clockwise turning 00066 n = n + 2; 00067 if (counter_cw >= 2500) 00068 { 00069 temp = counter_cw / 2500; 00070 current = counter_cw - 2500 * temp; 00071 t = n; 00072 Set_state(current); 00073 velocity = (temp * d * pi) / t; 00074 pc.printf("The wheel has run ");pc.printf("%f", distance); pc.printf("m."); 00075 pc.printf("The cw_speed is ");pc.printf("%f", velocity); pc.printf("m/s."); 00076 } 00077 //anti-clockwise turning 00078 else if (counter_cw >= -2500) 00079 { 00080 temp = counter_cw / 2500; 00081 current = counter_cw + 2500 * temp; 00082 t = n; 00083 Set_state(current); 00084 velocity = d * pi / t; 00085 pc.printf("The wheel has run ");pc.printf("%f", distance); pc.printf("m."); 00086 pc.printf("The cw_speed is ");pc.printf("%f", velocity); pc.printf("m/s."); 00087 } 00088 } 00089 00090 00091 00092 int main(){ 00093 setup(); 00094 pc.printf("start"); 00095 f.start(); 00096 while(1){ 00097 loop(); 00098 wait(2); 00099 pc.printf("%d %d \r\n", counter_cw, counter_ccw); 00100 } 00101 }
Generated on Thu Jul 14 2022 09:41:57 by
1.7.2