Diff: main.cpp

Revision:

1:cf9ba941d8c2

Parent:

0:9952fdc226ac

--- a/main.cpp	Wed Apr 17 15:40:42 2019 +0000
+++ b/main.cpp	Wed Apr 17 20:10:31 2019 +0000
@@ -1,31 +1,40 @@
 #include "mbed.h"
+#include "ContinuousServo.h"
+#include "Tach.h"
 Serial pc(USBTX,USBRX);
+
 //sonar sensor
 AnalogIn sonar(p19); //range sensor 9.8mV/inch
-
+//servo
+ContinuousServo left(p23);
+ContinuousServo right(p26);
+//encoders
+Tach tLeft(p17,64);
+Tach tRight(p13,64);
 int main()
 {
     while(1) {
-        float distance,x;
-        int num_iterations,i;
-        pc.scanf("%d",&num_iterations);//reads in values from the Matlab
+        float distance;
+        float stop_val;
+        int stop_dist;
+        int num_iterations;
+        int i;
+
+        pc.scanf("%d,%d",&num_iterations,&stop_dist);//reads in values from the Matlab
         for(i=1; i<=num_iterations; i++) { //will go until it meets the number of iterations
-            x = sonar.read();//takes the sensor redings and converts them to x
-            distance=x;
-            //if distance is == to desired distance than stop left and right wheels 
-            stopvalue = .033
-            if(x>stopvalue){
-                
+
+            distance = sonar.read();      //takes the sensor redings and converts them to "distance" variable
+            stop_val=.00275*stop_dist;    //takes the input desired stop distance and multiplies it by a constant to achieve a stop value
+
+            if(distance>stop_val) { //if distance is greater than desired stopping, then keep driving forward
                 left.speed(.5);
                 right.speed(.5);
-                
-                }else{
-                    left.stop();
-                    right.stop();
-                    }
-            
+            } else {                 //else stop left and right wheels
+                left.stop();
+                right.stop();
+            }
             wait(0.1);
-            pc.printf("%f\n",x);//mbed send out float values for Matlab
+            pc.printf("%f\n",distance);//mbed send out float values for Matlab
         }
     }
 }
\ No newline at end of file