Communicates with MATLAB to collect encoder data
Dependencies: QEI mbed mbedWSEsbc
main.cpp@0:6f89ab3c1588, 2018-02-12 (annotated)
- Committer:
- cjedwardz
- Date:
- Mon Feb 12 16:25:45 2018 +0000
- Revision:
- 0:6f89ab3c1588
Version 0
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
cjedwardz | 0:6f89ab3c1588 | 1 | #include "mbed.h" |
cjedwardz | 0:6f89ab3c1588 | 2 | #include "QEI.h" |
cjedwardz | 0:6f89ab3c1588 | 3 | //#include "mbedWSEsbc.h" |
cjedwardz | 0:6f89ab3c1588 | 4 | |
cjedwardz | 0:6f89ab3c1588 | 5 | Serial pc(USBTX, USBRX); |
cjedwardz | 0:6f89ab3c1588 | 6 | QEI encoder(p16, p17, NC, 3200); |
cjedwardz | 0:6f89ab3c1588 | 7 | |
cjedwardz | 0:6f89ab3c1588 | 8 | Ticker Tocker; |
cjedwardz | 0:6f89ab3c1588 | 9 | float TotalTime; |
cjedwardz | 0:6f89ab3c1588 | 10 | float Time; |
cjedwardz | 0:6f89ab3c1588 | 11 | float dt = 0.005; |
cjedwardz | 0:6f89ab3c1588 | 12 | float refresh = 0.01; |
cjedwardz | 0:6f89ab3c1588 | 13 | |
cjedwardz | 0:6f89ab3c1588 | 14 | float theta; |
cjedwardz | 0:6f89ab3c1588 | 15 | float omega; |
cjedwardz | 0:6f89ab3c1588 | 16 | int ticks; |
cjedwardz | 0:6f89ab3c1588 | 17 | |
cjedwardz | 0:6f89ab3c1588 | 18 | float theta_p; |
cjedwardz | 0:6f89ab3c1588 | 19 | float d_theta; |
cjedwardz | 0:6f89ab3c1588 | 20 | |
cjedwardz | 0:6f89ab3c1588 | 21 | float t2d = 0.1125; |
cjedwardz | 0:6f89ab3c1588 | 22 | |
cjedwardz | 0:6f89ab3c1588 | 23 | //Declare functions |
cjedwardz | 0:6f89ab3c1588 | 24 | void velocity(); |
cjedwardz | 0:6f89ab3c1588 | 25 | |
cjedwardz | 0:6f89ab3c1588 | 26 | int main() |
cjedwardz | 0:6f89ab3c1588 | 27 | { |
cjedwardz | 0:6f89ab3c1588 | 28 | int go = 0; |
cjedwardz | 0:6f89ab3c1588 | 29 | |
cjedwardz | 0:6f89ab3c1588 | 30 | // Set serial parameters |
cjedwardz | 0:6f89ab3c1588 | 31 | pc.baud(115200); |
cjedwardz | 0:6f89ab3c1588 | 32 | pc.format(8,SerialBase::None,1); |
cjedwardz | 0:6f89ab3c1588 | 33 | |
cjedwardz | 0:6f89ab3c1588 | 34 | // Initialize time |
cjedwardz | 0:6f89ab3c1588 | 35 | Time = 0.0; |
cjedwardz | 0:6f89ab3c1588 | 36 | // Attach ticker function |
cjedwardz | 0:6f89ab3c1588 | 37 | Tocker.attach(&velocity,dt); |
cjedwardz | 0:6f89ab3c1588 | 38 | |
cjedwardz | 0:6f89ab3c1588 | 39 | while(1) { |
cjedwardz | 0:6f89ab3c1588 | 40 | // Get "go" command from MATLAB |
cjedwardz | 0:6f89ab3c1588 | 41 | pc.scanf("%d,%f",&go, &TotalTime); |
cjedwardz | 0:6f89ab3c1588 | 42 | // Reset time |
cjedwardz | 0:6f89ab3c1588 | 43 | Time = 0.0; |
cjedwardz | 0:6f89ab3c1588 | 44 | if(go == 1) { |
cjedwardz | 0:6f89ab3c1588 | 45 | while(Time <= TotalTime) { |
cjedwardz | 0:6f89ab3c1588 | 46 | //pc.printf("Time: %f; Position: %f (deg); Velocity: %f (deg/s);\n", Time, theta, omega); |
cjedwardz | 0:6f89ab3c1588 | 47 | pc.printf("%f,%f,%f\n", Time, theta, omega); |
cjedwardz | 0:6f89ab3c1588 | 48 | wait(refresh); |
cjedwardz | 0:6f89ab3c1588 | 49 | } |
cjedwardz | 0:6f89ab3c1588 | 50 | } |
cjedwardz | 0:6f89ab3c1588 | 51 | } |
cjedwardz | 0:6f89ab3c1588 | 52 | } |
cjedwardz | 0:6f89ab3c1588 | 53 | |
cjedwardz | 0:6f89ab3c1588 | 54 | void velocity() |
cjedwardz | 0:6f89ab3c1588 | 55 | { |
cjedwardz | 0:6f89ab3c1588 | 56 | ticks = encoder.getPulses(); |
cjedwardz | 0:6f89ab3c1588 | 57 | theta = ticks*t2d; |
cjedwardz | 0:6f89ab3c1588 | 58 | Time = Time + dt; |
cjedwardz | 0:6f89ab3c1588 | 59 | // Angle in degrees |
cjedwardz | 0:6f89ab3c1588 | 60 | |
cjedwardz | 0:6f89ab3c1588 | 61 | //if (theta > 360.0) { |
cjedwardz | 0:6f89ab3c1588 | 62 | // theta = theta - 360.0; |
cjedwardz | 0:6f89ab3c1588 | 63 | //} else if (theta < 0.0) { |
cjedwardz | 0:6f89ab3c1588 | 64 | // theta = theta + 360.0; |
cjedwardz | 0:6f89ab3c1588 | 65 | //} |
cjedwardz | 0:6f89ab3c1588 | 66 | |
cjedwardz | 0:6f89ab3c1588 | 67 | d_theta = theta - theta_p; |
cjedwardz | 0:6f89ab3c1588 | 68 | omega = d_theta/dt; |
cjedwardz | 0:6f89ab3c1588 | 69 | |
cjedwardz | 0:6f89ab3c1588 | 70 | theta_p = theta; |
cjedwardz | 0:6f89ab3c1588 | 71 | } |