Sik Chiu Chow
/
arduino2
Arduino version 2
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
arduino2.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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