Revision 10:d14c702c1992, committed 2013-12-04

Comitter:

jaoramos

Date:

Wed Dec 04 04:11:41 2013 +0000

Parent:

9:4ff9849fc8f6

Child:

11:7e19e51b325d

Commit message:

Swing-up OK, LQR OK, transition not working

Changed in this revision

--- a/main.cpp	Wed Dec 04 03:20:34 2013 +0000
+++ b/main.cpp	Wed Dec 04 04:11:41 2013 +0000
@@ -52,11 +52,11 @@
 float lambda1 = 30, lambda2 = 30, lambda3 = 15;
 int index;
 int pulsesPend, pulsesMot;
-bool flag = 0;
+bool flag = 1;
 
 void saving(void const *args) {
     index = 0;
-    while ((index < BUFFER_SIZE)&&(flag == 1)&&(false))  {
+    while ((index < BUFFER_SIZE)&&(flag == 1))  {
         buffer[index] = theta1;
         buffer[index+1] = theta2;
         buffer[index+2] = dtheta1;
@@ -86,9 +86,10 @@
 float calcEnergy(void)
 {
 c2 = cos(theta2);
-return (I2x*dtheta2*dtheta2)/2.0 + (I2y*dtheta1*dtheta1)/2.0 + (L1*L1*dtheta1*dtheta1*m2)/2.0 + (dtheta1*dtheta1*l2*l2*m2)/2.0 + (dtheta2*dtheta2*l2*l2*m2)/2.0 
-- (I2y*dtheta1*dtheta1*c2*c2)/2.0 + (I2z*dtheta1*dtheta1*c2*c2)/2.0 + Ixz2*dtheta1*dtheta2*c2 + g*l2*m2*c2 - (dtheta1*dtheta1*l2*l2*m2*c2*c2)/2.0 
-- L1*dtheta1*dtheta2*l2*m2*c2;
+//return (I2x*dtheta2*dtheta2)/2.0 + (I2y*dtheta1*dtheta1)/2.0 + (L1*L1*dtheta1*dtheta1*m2)/2.0 + (dtheta1*dtheta1*l2*l2*m2)/2.0 + (dtheta2*dtheta2*l2*l2*m2)/2.0 
+//- (I2y*dtheta1*dtheta1*c2*c2)/2.0 + (I2z*dtheta1*dtheta1*c2*c2)/2.0 + Ixz2*dtheta1*dtheta2*c2 + g*l2*m2*c2 - (dtheta1*dtheta1*l2*l2*m2*c2*c2)/2.0 
+//- L1*dtheta1*dtheta2*l2*m2*c2;
+return (I2x*dtheta2*dtheta2)/2.0 + g*l2*m2*c2;
 }
 
 void setVoltage(float inputVoltage)
@@ -151,7 +152,7 @@
         {
             // z1 and z2 are in the beginning the same as the angle so that dtheta1 and dtheta2 are zero
             z1 = theta1;
-            z2 = theta2;
+            z2 = 1.001*theta2;
             firstTime = false;
         }
         //calculate dtheta1 
@@ -171,18 +172,15 @@
         z3 = z3 + dz3 * dt;
         mCurrent = z3;
         
-                //set pwm 
-        // ADD A SANITY CHECK ON THETA
-        if (cos(theta2) < 0.98) { 
+        //set pwm 
+        if (cos(theta2) < 0.96) { 
             flag = 1;
             currentEnergy = calcEnergy();
-            inputVoltage = -2.0*dtheta2*(0.0372 - currentEnergy); 
-                    
+            inputVoltage = -2.3*dtheta2*(0.0372 - currentEnergy);                   
             
         } else {
             flag = 1;
-            inputVoltage = 0.0;
-            //inputVoltage = -(k1*theta1 + k2*theta2 + k3*dtheta1 + k4*dtheta2 + k5*mCurrent);
+            inputVoltage = -(k1*theta1 + k2*theta2 + k3*dtheta1 + k4*dtheta2 + k5*mCurrent);
         }
         
         setVoltage(inputVoltage);