Arduino code converted to Mbed

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Encoder.cpp Source File

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 }