Diff: main.cpp

Revision:

0:1bcc2ae04a31

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Mar 05 01:16:24 2015 +0000
@@ -0,0 +1,41 @@
+//Integral
+#include "mbed.h"
+#include "Motor.h"
+Serial pc(USBTX,USBRX);//allows communication through the USB
+AnalogIn range(p19);
+float height;
+float rv;
+float dh;
+float ds;
+float er;
+float k=0.35;
+float ki=.03;
+float tgl=0.0;
+Motor m(p26, p29, p30);
+int count;
+int main()
+{
+
+    m.speed(0);
+    while(1) {
+        count=0;
+        m.speed(0);
+        rv=range;
+        height=(-75.7457492123552*pow(rv,2))+(90.6774745565381*rv)+(-4.00358328437282);
+        pc.printf("%f,%f\n",rv,height);
+        pc.printf("Enter desired height\n");
+        pc.scanf("%f",&dh);
+        while(count<600) {
+            rv=range;
+            height=(-75.7457492123552*pow(rv,2))+(90.6774745565381*rv)+(-4.00358328437282);
+            er=dh-height;
+            pc.printf("%f\n", er);
+            tgl=tgl+er*0.05;
+            ds=k*er+ki*tgl;
+            m.speed(-ds);
+            count=count+1;
+
+
+        }
+    }
+    }
\ No newline at end of file