Tachinit
Dependencies: mbed Tach ContinuousServo
main.cpp@1:66d0178e8f40, 2019-04-23 (annotated)
- Committer:
- m215910
- Date:
- Tue Apr 23 15:10:54 2019 +0000
- Revision:
- 1:66d0178e8f40
- Parent:
- 0:9acdbbfa4eb2
- Child:
- 2:35bc9b7f5756
drive straight and stop
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
m215910 | 1:66d0178e8f40 | 1 | //Satre, Gagnon, Ewing, Batten |
m215910 | 0:9acdbbfa4eb2 | 2 | //Tach Initial |
m215910 | 0:9acdbbfa4eb2 | 3 | |
m215910 | 0:9acdbbfa4eb2 | 4 | #include "mbed.h" |
m215910 | 0:9acdbbfa4eb2 | 5 | #include "ContinuousServo.h" |
m215910 | 0:9acdbbfa4eb2 | 6 | #include "Tach.h" |
m215910 | 0:9acdbbfa4eb2 | 7 | |
m215910 | 0:9acdbbfa4eb2 | 8 | |
m215910 | 0:9acdbbfa4eb2 | 9 | Serial pc(USBTX,USBRX); //Establish serial connection |
m215910 | 0:9acdbbfa4eb2 | 10 | |
m215910 | 0:9acdbbfa4eb2 | 11 | //speed |
m215910 | 0:9acdbbfa4eb2 | 12 | float wL, wR; |
m215910 | 1:66d0178e8f40 | 13 | float x = 0.2, y = 0.2, dist, realdist; |
m215910 | 0:9acdbbfa4eb2 | 14 | |
m215910 | 0:9acdbbfa4eb2 | 15 | ContinuousServo left(p23); |
m215910 | 0:9acdbbfa4eb2 | 16 | ContinuousServo right(p26); |
m215910 | 1:66d0178e8f40 | 17 | AnalogIn sonar(p19); //range sensor 9.8 mV/in |
m215910 | 0:9acdbbfa4eb2 | 18 | |
m215910 | 0:9acdbbfa4eb2 | 19 | //encoders |
m215910 | 0:9acdbbfa4eb2 | 20 | Tach tleft(p17,64); |
m215910 | 0:9acdbbfa4eb2 | 21 | Tach tright(p13,64); |
m215910 | 1:66d0178e8f40 | 22 | |
m215910 | 1:66d0178e8f40 | 23 | |
m215910 | 0:9acdbbfa4eb2 | 24 | int main() |
m215910 | 0:9acdbbfa4eb2 | 25 | { |
m215910 | 0:9acdbbfa4eb2 | 26 | |
m215910 | 0:9acdbbfa4eb2 | 27 | pc.baud(9600); //set baud rate |
m215910 | 0:9acdbbfa4eb2 | 28 | |
m215910 | 0:9acdbbfa4eb2 | 29 | while(1) { |
m215910 | 0:9acdbbfa4eb2 | 30 | |
m215910 | 1:66d0178e8f40 | 31 | left.speed(x); |
m215910 | 1:66d0178e8f40 | 32 | right.speed(-y); //right negative to go forward |
m215910 | 1:66d0178e8f40 | 33 | x = x; |
m215910 | 1:66d0178e8f40 | 34 | y = y; |
m215910 | 0:9acdbbfa4eb2 | 35 | |
m215910 | 0:9acdbbfa4eb2 | 36 | wL = tleft.getSpeed(); //speed of left wheel (rev/sec) |
m215910 | 0:9acdbbfa4eb2 | 37 | wR = tright.getSpeed(); //speed of right wheel (rev/sec) |
m215910 | 1:66d0178e8f40 | 38 | dist = sonar.read(); |
m215910 | 1:66d0178e8f40 | 39 | realdist = dist/0.00277; |
m215910 | 1:66d0178e8f40 | 40 | |
m215910 | 1:66d0178e8f40 | 41 | if (wL<wR) { |
m215910 | 1:66d0178e8f40 | 42 | x = x+0.03; |
m215910 | 1:66d0178e8f40 | 43 | y = y; } |
m215910 | 1:66d0178e8f40 | 44 | else if (wR<wL) { |
m215910 | 1:66d0178e8f40 | 45 | x = x; |
m215910 | 1:66d0178e8f40 | 46 | y = y+0.03;} |
m215910 | 1:66d0178e8f40 | 47 | else if (wR == wL) { |
m215910 | 1:66d0178e8f40 | 48 | x = x; |
m215910 | 1:66d0178e8f40 | 49 | y = y;} |
m215910 | 1:66d0178e8f40 | 50 | else if ((x || y) > 0.2) { |
m215910 | 1:66d0178e8f40 | 51 | x = 0.2; |
m215910 | 1:66d0178e8f40 | 52 | y = y;} |
m215910 | 1:66d0178e8f40 | 53 | |
m215910 | 1:66d0178e8f40 | 54 | if (realdist <= 8) { |
m215910 | 1:66d0178e8f40 | 55 | left.stop(); |
m215910 | 1:66d0178e8f40 | 56 | right.stop(); |
m215910 | 1:66d0178e8f40 | 57 | } |
m215910 | 1:66d0178e8f40 | 58 | pc.printf("%f, %f\n %f\n %f %f\n", wL, wR, realdist, x, y); |
m215910 | 0:9acdbbfa4eb2 | 59 | |
m215910 | 1:66d0178e8f40 | 60 | wait(.2); |
m215910 | 0:9acdbbfa4eb2 | 61 | |
m215910 | 0:9acdbbfa4eb2 | 62 | } //while |
m215910 | 0:9acdbbfa4eb2 | 63 | } //main |