Diff: main.cpp

Revision:

2:011e6115c77a

Parent:

1:5c05e0d08e61

Child:

3:967aee5fed5b

--- a/main.cpp	Mon Dec 02 00:09:50 2013 +0000
+++ b/main.cpp	Mon Dec 02 02:04:30 2013 +0000
@@ -1,7 +1,8 @@
 #include "mbed.h"
 #include "rtos.h"
 #include "QEI.h"
-
+#include <fstream>
+#include <iomanip>
 
 #define MOTOR_PPR 1200
 #define ENCODER_PPR 1024
@@ -24,6 +25,10 @@
 //Curent Measurement
 AnalogIn aIn(p16);    //pin 15 set as analog input.  Pins 15-20 can be used as analog inputs.
 
+//Motor direction and PWM
+DigitalOut dOut1(p5);
+DigitalOut dOut2(p7);
+PwmOut pwmOut(p21);
 
 // open a file for data logger
 LocalFileSystem local("local");
@@ -39,7 +44,7 @@
 float motor_conv = 2*OUR_PI/(float(MOTOR_PPR)*float(QUADRATURE_TYPE));
 
 float* buffer;
-float lambda = 30;
+float lambda1 = 30, lambda2 = 30, lambda3 = 15;
 int index;
 
 void saving(void const *args) {
@@ -58,12 +63,19 @@
 }
 
 void computing(void const *args) {
-    float z1 = 0.0, z2 = 0.0, dz1 = 0.0, dz2 = 0.0;
-    
+    float z1 = 0.0, z2 = 0.0, dz1 = 0.0, dz2 = 0.0, z3 = 0.0, dz3 = 0.0;
+    float freq = 1.0, dutyCycle;
     int pulsesPend, pulsesMot;
     while (true) {
         t = T.read();
         
+        //set pwm 
+        dutyCycle = (sin(freq*t));
+        dutyCycle = dutyCycle*dutyCycle;
+        //pc.printf("Duty%f\n\r",dutyCycle);
+        pwmOut.write(dutyCycle);
+        
+        //read current
         mCurrent = aIn.read()*MAX_VOLTAGE/VOLTS_PER_AMP; 
         pulsesPend = -encoder.getPulses();
         pulsesMot = motor.getPulses();
@@ -73,15 +85,20 @@
         theta1 = float(pulsesMot)*motor_conv;
         
         //calculate dtheta1 
-        dz1 = - lambda * z1 + lambda * theta1;
+        dz1 = - lambda1 * z1 + lambda1 * theta1;
         z1 = z1 + dz1 * dt;
         dtheta1 = dz1;
         
         //calculate dtheta2 
-        dz2 = - lambda * z2 + lambda * theta2;
+        dz2 = - lambda2 * z2 + lambda2 * theta2;
         z2 = z2 + dz2 * dt;
         dtheta2 = dz2;
-         
+        
+        //filter current 
+        dz3 = -lambda3 * z3 + lambda3 * mCurrent;
+        z3 = z3 + dz3 * dt;
+        mCurrent = z3;
+        
         t0 = t;
         Thread::wait(1);
     }
@@ -119,6 +136,12 @@
     Thread thrd2(computing,NULL,osPriorityRealtime);
     Thread thrd3(saving,NULL,osPriorityNormal);
     
+    //Run forward
+    pwmOut.period(0.0001); 
+    dOut1=1;
+    dOut2=0;
+    
+    
     pc.printf("Start!\r\n");
     pc.printf("Time: %f\r\n", t);
     while (t < 10.0) 
@@ -126,7 +149,8 @@
         pc.printf("Time: %f\r\n", t);
         Thread::wait(1000);
     }
-    pc.printf("Done!\r\n");
+    pc.printf("Done at Index: %d\r\n",index);
+    pwmOut.write(0.0);
     thrd2.terminate();
     thrd3.terminate();
     saveToFile();