Diff: main.cpp

Revision:

1:62e6eaeee186

Parent:

0:935cda04c01f

--- a/main.cpp	Thu Feb 12 15:53:02 2015 +0000
+++ b/main.cpp	Tue Mar 24 01:22:07 2015 +0000
@@ -1,40 +1,52 @@
 #include "mbed.h"
 #include "Motor.h"
-#include "time.h"
 
 Serial pc(USBTX,USBRX);
-AnalogIn ain(p17);
-Motor m(p25, p27, p28);
-float Lower_distance = 25.4;
-float Upper_distance = 50.8;
+AnalogIn ted(p17); // always check
+Motor m(p25, p27, p28); // always check
+float Lower_distance = 10; //floor 10
+float Upper_distance = 20; // floor 20
 float Desired_distance = Lower_distance;
 float distance;
 float analogVal = 0.0;
 float mospeed = 0.0;
+int time_count;
+float floor_error;
 
 Timer t;
 int main()
 {
+    // formating
     pc.baud(9600);
     pc.format(7,SerialBase::None,1);
+
     while(1) {
-        t.reset();
-        t.start();
-        while(t < 300) {
-            analogVal = ain.read();
-            pc.printf("%f\n", t.read());
-            pc.printf("%f", analogVal);
-            wait(.1);
-            distance = (2.6485*pow(analogVal,3)) - (29.915*pow(analogVal,2)) + (113.21*analogVal) - 85.547;
-            if ( distance < Desired_distance ) {
-                mospeed = .5;
-            }
-            if ( distance > Desired_distance) {
-                mospeed = -.5;
+        time_count = 0;
+        while(time_count < 300) {
+
+            analogVal = ted.read(); //reads in the sensor data and sets it to equal analogVal
+            pc.printf("%f ", t.read());
+            pc.printf("%f\n", distance);
+            wait(0.1); // pause
+            //distance = - .2439*analogVal*analogVal + 5.0129*analogVal - 1.8017; // equation which changes analogVal to equate to the distance
+            distance = -124.076*analogVal*analogVal + 121.51*analogVal - 8.02;
+            floor_error = Desired_distance - distance;
+
+            if ( floor_error < -0.5) {
+                mospeed = 2.5; //sets motor speed if distance is below desired distance
+            } else if ( floor_error > 0.5) {
+                mospeed = -2.5; // sets mototr speed if distance is above desired distance
             } else {
                 mospeed = 0;
             }
-            m.speed(mospeed);
+            m.speed(mospeed); // moves motor
+            time_count++;
         }
+        if ( Desired_distance == Lower_distance) {
+            Desired_distance = Upper_distance;
+        } else {
+            Desired_distance = Lower_distance;
+        }
+
     }
 }
\ No newline at end of file