Diff: main.cpp

Revision:

8:57c2b7c94ce8

Parent:

7:59613b7a1631

Child:

9:4ff9849fc8f6

diff -r 59613b7a1631 -r 57c2b7c94ce8 main.cpp
--- a/main.cpp	Wed Dec 04 01:49:01 2013 +0000
+++ b/main.cpp	Wed Dec 04 02:56:50 2013 +0000
@@ -3,6 +3,7 @@
 #include "QEI.h"
 #include <fstream>
 #include <iomanip>
+#include "stdlib.h"
 
 #define MOTOR_PPR 300
 #define ENCODER_PPR 1024
@@ -32,7 +33,7 @@
 
 // open a file for data logger
 LocalFileSystem local("local");
-float theta1, theta2, dtheta1, dtheta2;
+float theta1, theta2, dtheta1, dtheta2, dtheta2MvFil;
 float mCurrent = 0.0;
 float inputVoltage = 0.0;
 //int pulses0 = 0;
@@ -68,6 +69,27 @@
     }
 }
 
+float c2,
+g = 9.8,
+m2 = 0.0391 + 0.0259,
+L1 = (51+44.55)*0.001,
+l1 = -0.03478,
+L2 = 0.2983,
+l2 = 0.05847,
+I2x = 0.000534,
+I2y = 0.000841,
+I2z = 0.00031, 
+Ixz2 = -0.00024;
+float currentEnergy;
+
+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;
+}
+
 void setVoltage(float inputVoltage)
 {
     if(inputVoltage<0.0) {
@@ -82,6 +104,29 @@
     dutyCycle = (dutyCycle > 1.0)? 1.0 : dutyCycle;
     pwmOut.write(dutyCycle);
 }
+//
+//#define MOVING_AVERAGE_SIZE 10
+//
+//float movingAverage(float unfiltered)
+//{
+//    static std::deque<float> movAvgBuffer;              // empty deque of floats
+//    static float sum = 0.0;
+//    
+//    movAvgBuffer.push_front(unfiltered);
+//    sum += unfiltered;
+//    
+//    if( movAvgBuffer.size() <= MOVING_AVERAGE_SIZE)
+//    {
+//        return (sum/float(movAvgBuffer.size()));
+//    }
+//    else
+//    {
+//        sum -= movAvgBuffer.back();
+//        movAvgBuffer.pop_back();
+//    
+//        return (sum/float(MOVING_AVERAGE_SIZE));
+//    }
+//}
 
 void computing(void const *args) {
     float z1 = 0.0, z2 = 0.0, dz1 = 0.0, dz2 = 0.0, z3 = 0.0, dz3 = 0.0; 
@@ -89,17 +134,7 @@
     while (true ) {
         t = T.read();
         
-        //set pwm 
-        // ADD A SANITY CHECK ON THETA
-        if (cos(theta2) < 0.98) {
-            flag = 0;
-            inputVoltage = 0.0;
-        } else {
-            flag = 1;
-            inputVoltage = -(k1*theta1 + k2*theta2 + k3*dtheta1 + k4*dtheta2 + k5*mCurrent);
-        }
-        setVoltage(inputVoltage);
-        
+       
         //read current
         mCurrent = aIn.read()*MAX_VOLTAGE/VOLTS_PER_AMP; 
         if(dOut1 == 0)
@@ -121,11 +156,28 @@
         z2 = z2 + dz2 * dt;
         dtheta2 = dz2;
         
+        //dtheta2MvFil = movingAverage(dtheta2);
+        
         //filter current 
         dz3 = -lambda3 * z3 + lambda3 * mCurrent;
         z3 = z3 + dz3 * dt;
         mCurrent = z3;
         
+                //set pwm 
+        // ADD A SANITY CHECK ON THETA
+        if (cos(theta2) < 0.98) {
+            flag = 0;
+            currentEnergy = calcEnergy();
+            inputVoltage = -2.0*dtheta2*(0.0372 - currentEnergy); 
+                    
+            
+        } else {
+            flag = 1;
+            inputVoltage = 0.0;
+            //inputVoltage = -(k1*theta1 + k2*theta2 + k3*dtheta1 + k4*dtheta2 + k5*mCurrent);
+        }
+        setVoltage(inputVoltage);
+        
         t0 = t;
         Thread::wait(1);
     }
@@ -170,10 +222,12 @@
     
     pc.printf("Start!\r\n");
     pc.printf("Time: %f\r\n", t);
+    
     while (t < PROGRAM_RUNTIME) 
     {
-        //pc.printf("Time: %f\r\n", t);
-        Thread::wait(1000);
+        pc.printf("at time: %f energy: %f\n",t, currentEnergy);
+        
+        Thread::wait(200);
     }
     pc.printf("Done at Index: %d\r\n",index);
     pwmOut.write(0.0);