Diff: main.cpp

Revision:

4:8fcaff7801b0

Parent:

3:967aee5fed5b

Child:

5:d41998e421ed

--- a/main.cpp	Mon Dec 02 02:22:37 2013 +0000
+++ b/main.cpp	Mon Dec 02 04:38:30 2013 +0000
@@ -4,7 +4,7 @@
 #include <fstream>
 #include <iomanip>
 
-#define MOTOR_PPR 1200
+#define MOTOR_PPR 300
 #define ENCODER_PPR 1024
 
 #define QUADRATURE_TYPE  2
@@ -39,6 +39,7 @@
 //int deltaPulses;
 float t0 = 0.0;
 float t = 0.0, dt;
+float k1 = -0.0316, k2 = 9.7076, k3 = -0.4095, k4 = 1.2340, k5 = 0.0410;
 
 float encoder_conv = 2*OUR_PI/(float(ENCODER_PPR)*float(QUADRATURE_TYPE));
 float motor_conv = 2*OUR_PI/(float(MOTOR_PPR)*float(QUADRATURE_TYPE));
@@ -46,18 +47,18 @@
 float* buffer;
 float lambda1 = 30, lambda2 = 30, lambda3 = 15;
 int index;
+int pulsesPend, pulsesMot;
 
 void saving(void const *args) {
     index = 0;
-    while (true) {
+    while (index < buffer_size) {
         buffer[index] = theta1;
         buffer[index+1] = theta2;
         buffer[index+2] = dtheta1;
         buffer[index+3] = dtheta2;
         buffer[index+4] = mCurrent;
         buffer[index+5] = t;
-        
-        index = index+DATA_COLS;
+        index = index + DATA_COLS;
         Thread::wait(20);
     }
 }
@@ -79,14 +80,16 @@
 
 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 freq = 2.0, inputVoltage;
-    int pulsesPend, pulsesMot;
+    float inputVoltage; 
+    
     while (true) {
         t = T.read();
         
         //set pwm 
-        inputVoltage = MAX_VOLTAGE * (sin(freq*t));
-        //pc.printf("Duty%f\n\r",dutyCycle);
+        // ADD A SANITY CHECK ON THETA
+        inputVoltage = -(k1*theta1 + k2*theta2 + k3*dtheta1 + k4*dtheta2 + k5*mCurrent); 
+        if (cos(theta2) < 0.98) 
+            inputVoltage = 0.0;
         setVoltage(inputVoltage);
         
         //read current
@@ -97,7 +100,7 @@
         pulsesMot = motor.getPulses();
         
         dt = t - t0; //time difference
-        theta2 = float(pulsesPend)*encoder_conv;
+        theta2 = float(pulsesPend)*encoder_conv + OUR_PI;
         theta1 = float(pulsesMot)*motor_conv;
         
         //calculate dtheta1 
@@ -157,12 +160,11 @@
     dOut1=1;
     dOut2=0;
     
-    
     pc.printf("Start!\r\n");
     pc.printf("Time: %f\r\n", t);
     while (t < 10.0) 
     {
-        pc.printf("Time: %f\r\n", t);
+        //pc.printf("Time: %f\r\n", t);
         Thread::wait(1000);
     }
     pc.printf("Done at Index: %d\r\n",index);