Diff: main.cpp

Revision:

9:4ff9849fc8f6

Parent:

8:57c2b7c94ce8

Child:

10:d14c702c1992

--- a/main.cpp	Wed Dec 04 02:56:50 2013 +0000
+++ b/main.cpp	Wed Dec 04 03:20:34 2013 +0000
@@ -80,6 +80,7 @@
 I2y = 0.000841,
 I2z = 0.00031, 
 Ixz2 = -0.00024;
+
 float currentEnergy;
 
 float calcEnergy(void)
@@ -129,7 +130,8 @@
 //}
 
 void computing(void const *args) {
-    float z1 = 0.0, z2 = 0.0, dz1 = 0.0, dz2 = 0.0, z3 = 0.0, dz3 = 0.0; 
+    float z1=0.0, z2=0.0, dz1 = 0.0, dz2 = 0.0, z3 = 0.0, dz3 = 0.0; 
+    bool firstTime = true;
     
     while (true ) {
         t = T.read();
@@ -145,7 +147,13 @@
         dt = t - t0; //time difference
         theta2 = float(pulsesPend)*encoder_conv + OUR_PI;
         theta1 = float(pulsesMot)*motor_conv;
-        
+        if(firstTime)
+        {
+            // z1 and z2 are in the beginning the same as the angle so that dtheta1 and dtheta2 are zero
+            z1 = theta1;
+            z2 = theta2;
+            firstTime = false;
+        }
         //calculate dtheta1 
         dz1 = - lambda1 * z1 + lambda1 * theta1;
         z1 = z1 + dz1 * dt;
@@ -165,8 +173,8 @@
         
                 //set pwm 
         // ADD A SANITY CHECK ON THETA
-        if (cos(theta2) < 0.98) {
-            flag = 0;
+        if (cos(theta2) < 0.98) { 
+            flag = 1;
             currentEnergy = calcEnergy();
             inputVoltage = -2.0*dtheta2*(0.0372 - currentEnergy); 
                     
@@ -176,6 +184,7 @@
             inputVoltage = 0.0;
             //inputVoltage = -(k1*theta1 + k2*theta2 + k3*dtheta1 + k4*dtheta2 + k5*mCurrent);
         }
+        
         setVoltage(inputVoltage);
         
         t0 = t;
@@ -214,6 +223,8 @@
     T.start();
     pwmOut.period(0.0001); 
     
+    Thread::wait(2000);
+    
     Thread thrd2(computing,NULL,osPriorityRealtime);
     pc.printf("started computing thread!\r\n");
     Thread thrd3(saving,NULL,osPriorityNormal);
@@ -225,7 +236,7 @@
     
     while (t < PROGRAM_RUNTIME) 
     {
-        pc.printf("at time: %f energy: %f\n",t, currentEnergy);
+        //pc.printf("at time: %f energy: %f\n",t, currentEnergy);
         
         Thread::wait(200);
     }