Sik Chiu Chow
/
arduino2
Arduino version 2
arduino2.cpp@1:4df9f94facd5, 2021-11-02 (annotated)
- Committer:
- ea78anana
- Date:
- Tue Nov 02 15:27:52 2021 +0000
- Revision:
- 1:4df9f94facd5
- Parent:
- 0:c9022327ece6
- Child:
- 2:46365fee3503
Encoder modify from arduino
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ea78anana | 0:c9022327ece6 | 1 | #include "mbed.h" |
ea78anana | 0:c9022327ece6 | 2 | #include "arduino.h" |
ea78anana | 0:c9022327ece6 | 3 | |
ea78anana | 1:4df9f94facd5 | 4 | #define PinA A1 //A phase |
ea78anana | 1:4df9f94facd5 | 5 | #define PinZ A3 //Z phase |
ea78anana | 1:4df9f94facd5 | 6 | #define PinB A2 //B phase |
ea78anana | 0:c9022327ece6 | 7 | |
ea78anana | 0:c9022327ece6 | 8 | #define HIGH 1 |
ea78anana | 0:c9022327ece6 | 9 | #define LOW 0 |
ea78anana | 0:c9022327ece6 | 10 | |
ea78anana | 0:c9022327ece6 | 11 | //Initialize Variable |
ea78anana | 0:c9022327ece6 | 12 | unsigned long time1 = 0; // Time |
ea78anana | 0:c9022327ece6 | 13 | float time_cw; |
ea78anana | 0:c9022327ece6 | 14 | float time_ccw; |
ea78anana | 0:c9022327ece6 | 15 | int counter = 0; |
ea78anana | 1:4df9f94facd5 | 16 | const float d = 0.06; //Radius of the wheel |
ea78anana | 0:c9022327ece6 | 17 | const float pi = 3.141592654;//PI |
ea78anana | 0:c9022327ece6 | 18 | int num = 0;//number of turns |
ea78anana | 0:c9022327ece6 | 19 | double t;//time per turn |
ea78anana | 0:c9022327ece6 | 20 | float velocity; |
ea78anana | 1:4df9f94facd5 | 21 | double time3;//Time of phase Z detected, use for calculate the velocity |
ea78anana | 1:4df9f94facd5 | 22 | Timer f; |
ea78anana | 0:c9022327ece6 | 23 | |
ea78anana | 0:c9022327ece6 | 24 | DigitalIn a11(PinA); |
ea78anana | 0:c9022327ece6 | 25 | DigitalIn a12(PinB); |
ea78anana | 1:4df9f94facd5 | 26 | InterruptIn a14(PinA); |
ea78anana | 1:4df9f94facd5 | 27 | InterruptIn a13(PinZ); |
ea78anana | 1:4df9f94facd5 | 28 | |
ea78anana | 0:c9022327ece6 | 29 | Serial pc(USBTX, USBRX); |
ea78anana | 0:c9022327ece6 | 30 | |
ea78anana | 1:4df9f94facd5 | 31 | void Encode() |
ea78anana | 1:4df9f94facd5 | 32 | { |
ea78anana | 0:c9022327ece6 | 33 | |
ea78anana | 1:4df9f94facd5 | 34 | //Ignore noise pulses |
ea78anana | 1:4df9f94facd5 | 35 | |
ea78anana | 1:4df9f94facd5 | 36 | //When the two reading is bigger than 5ms then count it as correct pulse |
ea78anana | 1:4df9f94facd5 | 37 | //uint32_t m = f.read_ms(); |
ea78anana | 1:4df9f94facd5 | 38 | |
ea78anana | 1:4df9f94facd5 | 39 | //if ((m - time1) > 5) |
ea78anana | 1:4df9f94facd5 | 40 | |
ea78anana | 1:4df9f94facd5 | 41 | { |
ea78anana | 1:4df9f94facd5 | 42 | |
ea78anana | 1:4df9f94facd5 | 43 | //When Phase A is falling will be detected |
ea78anana | 1:4df9f94facd5 | 44 | |
ea78anana | 1:4df9f94facd5 | 45 | if ((a11 == LOW) && (a12 == HIGH)) |
ea78anana | 1:4df9f94facd5 | 46 | |
ea78anana | 1:4df9f94facd5 | 47 | { pc.printf("backward "); |
ea78anana | 1:4df9f94facd5 | 48 | |
ea78anana | 1:4df9f94facd5 | 49 | counter--; |
ea78anana | 1:4df9f94facd5 | 50 | |
ea78anana | 1:4df9f94facd5 | 51 | } |
ea78anana | 1:4df9f94facd5 | 52 | |
ea78anana | 1:4df9f94facd5 | 53 | else |
ea78anana | 1:4df9f94facd5 | 54 | |
ea78anana | 1:4df9f94facd5 | 55 | { pc.printf("fordward "); |
ea78anana | 1:4df9f94facd5 | 56 | |
ea78anana | 1:4df9f94facd5 | 57 | counter++; |
ea78anana | 1:4df9f94facd5 | 58 | |
ea78anana | 1:4df9f94facd5 | 59 | } |
ea78anana | 1:4df9f94facd5 | 60 | |
ea78anana | 1:4df9f94facd5 | 61 | } |
ea78anana | 1:4df9f94facd5 | 62 | |
ea78anana | 1:4df9f94facd5 | 63 | //time1 = m; |
ea78anana | 1:4df9f94facd5 | 64 | |
ea78anana | 1:4df9f94facd5 | 65 | } |
ea78anana | 1:4df9f94facd5 | 66 | |
ea78anana | 1:4df9f94facd5 | 67 | void setup(){ |
ea78anana | 1:4df9f94facd5 | 68 | a11.mode(PullUp); |
ea78anana | 1:4df9f94facd5 | 69 | a12.mode(PullUp); |
ea78anana | 1:4df9f94facd5 | 70 | a13.fall(&Encode); |
ea78anana | 1:4df9f94facd5 | 71 | //a14.rise(&Encode); |
ea78anana | 1:4df9f94facd5 | 72 | //a14.fall(&Encode); |
ea78anana | 1:4df9f94facd5 | 73 | } |
ea78anana | 1:4df9f94facd5 | 74 | |
ea78anana | 1:4df9f94facd5 | 75 | void Set_state(){ |
ea78anana | 1:4df9f94facd5 | 76 | counter = 0; |
ea78anana | 1:4df9f94facd5 | 77 | uint32_t p = f.read_ms(); |
ea78anana | 1:4df9f94facd5 | 78 | time3 = p;//Time when Z phase detected |
ea78anana | 1:4df9f94facd5 | 79 | } |
ea78anana | 0:c9022327ece6 | 80 | |
ea78anana | 0:c9022327ece6 | 81 | void loop() |
ea78anana | 0:c9022327ece6 | 82 | |
ea78anana | 0:c9022327ece6 | 83 | { |
ea78anana | 0:c9022327ece6 | 84 | double distance; |
ea78anana | 0:c9022327ece6 | 85 | //clockwise turning |
ea78anana | 1:4df9f94facd5 | 86 | uint32_t n = f.read_ms(); |
ea78anana | 1:4df9f94facd5 | 87 | if (counter == 25) |
ea78anana | 0:c9022327ece6 | 88 | { |
ea78anana | 0:c9022327ece6 | 89 | // Printf("ok");//Testing |
ea78anana | 1:4df9f94facd5 | 90 | num = num + 25; |
ea78anana | 1:4df9f94facd5 | 91 | Set_state(); |
ea78anana | 1:4df9f94facd5 | 92 | t = n- time3; |
ea78anana | 0:c9022327ece6 | 93 | t = t / 1000; |
ea78anana | 0:c9022327ece6 | 94 | distance = num * d * pi; |
ea78anana | 0:c9022327ece6 | 95 | velocity = d * pi / t; |
ea78anana | 0:c9022327ece6 | 96 | pc.printf("The wheel has run ");pc.printf("%f", distance); pc.printf("m."); |
ea78anana | 0:c9022327ece6 | 97 | pc.printf("The cw_speed is ");pc.printf("%f", velocity); pc.printf("m/s."); |
ea78anana | 0:c9022327ece6 | 98 | pc.printf("The time is ");pc.printf("%f", t); pc.printf("s."); |
ea78anana | 0:c9022327ece6 | 99 | } |
ea78anana | 0:c9022327ece6 | 100 | //anti-clockwise turning |
ea78anana | 1:4df9f94facd5 | 101 | else if (counter == -25) |
ea78anana | 0:c9022327ece6 | 102 | { |
ea78anana | 0:c9022327ece6 | 103 | // Printf("ok");//Testing |
ea78anana | 1:4df9f94facd5 | 104 | num = num + 25; |
ea78anana | 1:4df9f94facd5 | 105 | Set_state(); |
ea78anana | 1:4df9f94facd5 | 106 | t = n - time3; |
ea78anana | 0:c9022327ece6 | 107 | t = t / 1000; |
ea78anana | 0:c9022327ece6 | 108 | distance = num * d * pi; |
ea78anana | 0:c9022327ece6 | 109 | velocity = d * pi / t; |
ea78anana | 0:c9022327ece6 | 110 | pc.printf("The wheel has run ");pc.printf("%f", distance); pc.printf("m."); |
ea78anana | 0:c9022327ece6 | 111 | pc.printf("The cw_speed is ");pc.printf("%f", velocity); pc.printf("m/s."); |
ea78anana | 0:c9022327ece6 | 112 | pc.printf("The time is ");pc.printf("%f", t); pc.printf("s."); |
ea78anana | 0:c9022327ece6 | 113 | } |
ea78anana | 1:4df9f94facd5 | 114 | //pc.printf("%d \n",counter); |
ea78anana | 0:c9022327ece6 | 115 | } |
ea78anana | 0:c9022327ece6 | 116 | |
ea78anana | 0:c9022327ece6 | 117 | |
ea78anana | 0:c9022327ece6 | 118 | |
ea78anana | 1:4df9f94facd5 | 119 | int main(void){ |
ea78anana | 1:4df9f94facd5 | 120 | setup(); |
ea78anana | 1:4df9f94facd5 | 121 | pc.printf("start"); |
ea78anana | 1:4df9f94facd5 | 122 | f.start(); |
ea78anana | 1:4df9f94facd5 | 123 | while(1){ |
ea78anana | 1:4df9f94facd5 | 124 | loop(); |
ea78anana | 0:c9022327ece6 | 125 | } |
ea78anana | 0:c9022327ece6 | 126 | } |