Tachinit
Dependencies: mbed Tach ContinuousServo
Diff: main.cpp
- Revision:
- 1:66d0178e8f40
- Parent:
- 0:9acdbbfa4eb2
- Child:
- 2:35bc9b7f5756
diff -r 9acdbbfa4eb2 -r 66d0178e8f40 main.cpp --- a/main.cpp Wed Apr 17 14:45:09 2019 +0000 +++ b/main.cpp Tue Apr 23 15:10:54 2019 +0000 @@ -1,4 +1,4 @@ -//Satre Gagnon, Ewing, Batten +//Satre, Gagnon, Ewing, Batten //Tach Initial #include "mbed.h" @@ -10,14 +10,17 @@ //speed float wL, wR; +float x = 0.2, y = 0.2, dist, realdist; ContinuousServo left(p23); ContinuousServo right(p26); +AnalogIn sonar(p19); //range sensor 9.8 mV/in //encoders Tach tleft(p17,64); Tach tright(p13,64); - + + int main() { @@ -25,14 +28,36 @@ while(1) { - left.speed(0.5); - right.speed(-0.5); //right negative to go forward + left.speed(x); + right.speed(-y); //right negative to go forward + x = x; + y = y; wL = tleft.getSpeed(); //speed of left wheel (rev/sec) wR = tright.getSpeed(); //speed of right wheel (rev/sec) + dist = sonar.read(); + realdist = dist/0.00277; + + if (wL<wR) { + x = x+0.03; + y = y; } + else if (wR<wL) { + x = x; + y = y+0.03;} + else if (wR == wL) { + x = x; + y = y;} + else if ((x || y) > 0.2) { + x = 0.2; + y = y;} + + if (realdist <= 8) { + left.stop(); + right.stop(); + } + pc.printf("%f, %f\n %f\n %f %f\n", wL, wR, realdist, x, y); - pc.printf("%f, %f\n", wL, wR); - wait(1); + wait(.2); } //while } //main \ No newline at end of file