![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Arduino version 2
arduino2.cpp
- Committer:
- ea78anana
- Date:
- 2021-11-17
- Revision:
- 2:46365fee3503
- Parent:
- 1:4df9f94facd5
File content as of revision 2:46365fee3503:
#include "mbed.h" #include "arduino.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; double time3;//Time of phase Z detected, use for calculate the velocity Timer f; DigitalIn a11(PinA); DigitalIn a12(PinB); //InterruptIn a11(PinA); InterruptIn a13(PinZ); //InterruptIn a12(PinB); Serial pc(USBTX, USBRX); void Encode0() { //Ignore noise pulses //When the two reading is bigger than 10ms then count it as correct pulse //uint32_t m = f.read_ms(); //if ((m - time1) > 0.2) //{ //When Phase A is falling will be detected if((current == 1) && (a11 == HIGH) && (a12 == LOW)) { counter_cw++; pc.printf("Foward %d \r\n", counter_cw); } else if((current == 2) && (a11 == LOW) && (a12 == HIGH)) { counter_ccw++; 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.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_cw = 0; counter_ccw = 0; uint32_t p = f.read_ms(); time3 = p;//Time when Z phase detected } void loop() { double distance; //clockwise turning uint32_t n = f.read_ms(); if (counter_cw == 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."); } //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); } int main(void){ setup(); pc.printf("start"); f.start(); while(1){ uint32_t m = f.read(); if(m > 0.5){ current = 0; } //loop(); } }