Diff: main.cpp

Revision:

0:99b19c26b13e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Mar 02 14:39:18 2015 +0000
@@ -0,0 +1,53 @@
+// Proportional Motor Control Code
+// 19 FEB 15
+// Changdae Hahm and Charles Stabler
+
+#include "mbed.h"
+#include "Motor.h"
+
+Motor m(p26,p29,p30);      // motor control
+AnalogIn ted(p19);         // sensor pin
+Serial pc(USBTX,USBRX);    // serial comms
+Timer t;
+
+float mspd;
+float k = 0.4;                 // proportional value
+float err;
+float Hold_value = 0.2;
+float height;
+float read;
+int time_count = 0;
+int i = 0;
+
+int main() {
+    
+    pc.baud(9600);
+    pc.format(7,SerialBase::None,1);
+    t.start();
+    
+    while(1) {
+        while(i == 0){               
+            read = ted.read();  // reads in sensor values
+            height = ((495.425114818726*pow(read,3)) - (734.715379566225*pow(read,2)) + (358.613836921017*read) - 35.5785628052144)-3.5; // height calculation based on sensor calibration
+            pc.printf("%f,%f\n", height,t.read());   // prints height and time for TeraTerm
+            err = 18.0 - height;
+            mspd = (k * err) + Hold_value;  // sets motor speed based on distance from desired floor, most likely value is positive 
+            m.speed(mspd);
+            if(abs(err) <= 0.2){   // breaks loop when elevator reaches desired floor 
+                i = 1;
+            }
+        }
+            
+        while(i == 1){    
+            read = ted.read();  // reading sensor values
+            height = ((495.425114818726*pow(read,3)) - (734.715379566225*pow(read,2)) + (358.613836921017*read) - 35.5785628052144)+1.5;  // height calculations based on sensor calibration
+            pc.printf("%f,%f\n", height,t.read());
+            err = 7.0 - height;
+            mspd = (k * err) + Hold_value;  // sets motor speed based on distance from floor, most likely value is negative
+            m.speed(mspd);    
+            if(abs(err) <= 0.2){   // breaks loop when desired floor is reached
+                i = 0;
+            }
+        }
+    }
+}