Sp2019_EW202_3321_1 / Mbed 2 deprecated tach_initial

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?

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