Sik Chiu Chow / Mbed 2 deprecated arduino2

Arduino version 2

Dependencies:   mbed arduino

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?

UserRevisionLine numberNew 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 }