Sp2019_EW202_3321_1 / Mbed 2 deprecated tach_initial

Tachinit

Dependencies:   mbed Tach ContinuousServo

main.cpp@2:35bc9b7f5756, 2019-04-23 (annotated)

Committer:
m215910
Date:
Tue Apr 23 15:45:32 2019 +0000
Revision:
2:35bc9b7f5756
Parent:
1:66d0178e8f40 
changes still not right

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 2:35bc9b7f5756 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 2:35bc9b7f5756 32 right.speed(y); //right negative to go forward
m215910 0:9acdbbfa4eb2 33
m215910 0:9acdbbfa4eb2 34 wL = tleft.getSpeed(); //speed of left wheel (rev/sec)
m215910 0:9acdbbfa4eb2 35 wR = tright.getSpeed(); //speed of right wheel (rev/sec)
m215910 1:66d0178e8f40 36 dist = sonar.read();
m215910 1:66d0178e8f40 37 realdist = dist/0.00277;
m215910 1:66d0178e8f40 38
m215910 1:66d0178e8f40 39 if (wL<wR) {
m215910 2:35bc9b7f5756 40 x = x+0.01;
m215910 1:66d0178e8f40 41 y = y; }
m215910 1:66d0178e8f40 42 else if (wR<wL) {
m215910 2:35bc9b7f5756 43 x = x-0.01;
m215910 2:35bc9b7f5756 44 y = y;}
m215910 1:66d0178e8f40 45 else if (wR == wL) {
m215910 1:66d0178e8f40 46 x = x;
m215910 1:66d0178e8f40 47 y = y;}
m215910 2:35bc9b7f5756 48
m215910 2:35bc9b7f5756 49 if ((x > 0.3) || (y < -0.3)) {
m215910 2:35bc9b7f5756 50 x = 0.3;
m215910 1:66d0178e8f40 51 y = y;}
m215910 1:66d0178e8f40 52
m215910 1:66d0178e8f40 53 if (realdist <= 8) {
m215910 1:66d0178e8f40 54 left.stop();
m215910 1:66d0178e8f40 55 right.stop();
m215910 1:66d0178e8f40 56 }
m215910 2:35bc9b7f5756 57 pc.printf("%f, %f, %f, %f, %f\n", wL, wR, realdist, x, y);
m215910 0:9acdbbfa4eb2 58
m215910 1:66d0178e8f40 59 wait(.2);
m215910 0:9acdbbfa4eb2 60
m215910 0:9acdbbfa4eb2 61 } //while
m215910 0:9acdbbfa4eb2 62 } //main