Diff: main.cpp

Revision:

1:38c836c4bc0f

Parent:

0:19db2de32450

--- a/main.cpp	Wed Mar 09 03:36:06 2016 +0000
+++ b/main.cpp	Thu Mar 10 19:52:42 2016 +0000
@@ -6,14 +6,14 @@
 Motor RickyBobby(p25,p27,p28);                          //declares output to motor
 
 int go,cnt,pwm;                                         //These two variables are integers
-float number,counter,x,mesHeight,desHeight,err;
+float number,counter,x,mesHeight,desHeight,err,Kp,dutyCyc,intErr,Ki;
 int main()
 {
     pc.baud(115200);                                    //sets baud rate
 
     while(1) {                                          //Infinite loop
         cnt=0;
-        number=60;
+        number=100;
 
         //these two lines communicate from the mbed to the teraterm window to collect
         //desired values
@@ -29,28 +29,58 @@
                 x=dale.read();
                 mesHeight = ((267.3918*x*x*x) + (-431.3231*x*x) + 236.6525*x + -22.2608);
                 err = desHeight-mesHeight;
+                Kp = 1.8;       
+ 
+/*                
+                //This is the Integral Controller
+                intErr = err*cnt;
+                dutyCyc = Kp*err + Ki*intErr;
+                pwm = dutyCyc;
+                RickyBobby.speed(dutyCyc);
+                wait(.05);
+*/
+
+                //This is the Proportional Controller
+               
+                dutyCyc = (Kp*err);
+                RickyBobby.speed(dutyCyc);
+                wait(.05);
+                pwm = dutyCyc;
+
+
+                if (pwm > 1) {
+                    pwm = 1;
+                    //dutyCyc = 1;
+                }
+
+                else if (pwm < -1) {
+                    pwm = -1;
+                    //dutyCyc = -1;
+                }
+
+
                 //pc.printf("mesHeight = %f\r\n", mesHeight);
 
-                if (err > 0) {
+                //This is the Logic Controller
+/*                if (err > 0) {              
                     RickyBobby.speed(1);
                     pwm = 1;
                     wait(.05);
-                } 
+                }
                 else if (err < 0) {
                     RickyBobby.speed(-1);
                     pwm = -1;
                     wait(.05);
                 }
-
                 else {
                     RickyBobby.speed(0);
                     pwm = 0;
                 }
-
+*/
                 cnt=cnt+1;
                 wait(.05);
                 pc.printf("%d,%f,%d\r\n",cnt,mesHeight,pwm);
-                
+
             }
             RickyBobby.speed(0);
             go = 0;