Diff: main.cpp

Revision:

1:5c05e0d08e61

Parent:

0:9f2b0ea63eac

Child:

2:011e6115c77a

--- a/main.cpp	Sun Dec 01 23:00:51 2013 +0000
+++ b/main.cpp	Mon Dec 02 00:09:50 2013 +0000
@@ -2,51 +2,86 @@
 #include "rtos.h"
 #include "QEI.h"
 
-#define NR_SAMPLES 4000
+
+#define MOTOR_PPR 1200
+#define ENCODER_PPR 1024
+
+#define QUADRATURE_TYPE  2
+#define OUR_PI  3.141592653589793
+#define DATA_COLS 6
+#define NR_SAMPLES 1500
+#define buffer_size 3500
+#define MAX_VOLTAGE 3.3
+#define VOLTS_PER_AMP 0.14
+
 
 Serial pc(USBTX, USBRX);
 
-QEI encoder(p29, p30, NC, 1024);
+QEI encoder(p29, p30, NC, ENCODER_PPR);
+QEI motor(p25, p26, NC, MOTOR_PPR);
 Timer T;
 
+//Curent Measurement
+AnalogIn aIn(p16);    //pin 15 set as analog input.  Pins 15-20 can be used as analog inputs.
+
+
 // open a file for data logger
 LocalFileSystem local("local");
-
-float angle = 0.0;
-int pulses0 = 0;
-int deltaPulses;
+//const int  buffer_size = DATA_COLS * NR_SAMPLES;
+float theta1, theta2, dtheta1, dtheta2;
+float mCurrent = 0.0;
+//int pulses0 = 0;
+//int deltaPulses;
 float t0 = 0.0;
-float t = 0.0, dt, vel;
-int curPulses;
-float conversion = 360.0/(1024.0*2.0);
+float t = 0.0, dt;
+
+float encoder_conv = 2*OUR_PI/(float(ENCODER_PPR)*float(QUADRATURE_TYPE));
+float motor_conv = 2*OUR_PI/(float(MOTOR_PPR)*float(QUADRATURE_TYPE));
+
 float* buffer;
-
+float lambda = 30;
 int index;
 
 void saving(void const *args) {
     index = 0;
     while (true) {
-        //fprintf(fp, "0.0\n");
-        //fprintf(fp, "%f, %f, %f\n", angle, vel, t); 
-        //printf("Vel is: %f, dt = %f\r\n", vel, dt);
-        buffer[index] = angle;
-        buffer[index+1] = t;
-        index = index+2;
-        Thread::wait(10);
+        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;
+        Thread::wait(20);
     }
 }
 
 void computing(void const *args) {
+    float z1 = 0.0, z2 = 0.0, dz1 = 0.0, dz2 = 0.0;
+    
+    int pulsesPend, pulsesMot;
     while (true) {
         t = T.read();
-        dt = t - t0;
-        curPulses = encoder.getPulses();
-        angle = float(curPulses)*conversion;
+        
+        mCurrent = aIn.read()*MAX_VOLTAGE/VOLTS_PER_AMP; 
+        pulsesPend = -encoder.getPulses();
+        pulsesMot = motor.getPulses();
+        
+        dt = t - t0; //time difference
+        theta2 = float(pulsesPend)*encoder_conv;
+        theta1 = float(pulsesMot)*motor_conv;
         
-        deltaPulses = curPulses - pulses0;
-        vel = float(deltaPulses)/dt*conversion;
+        //calculate dtheta1 
+        dz1 = - lambda * z1 + lambda * theta1;
+        z1 = z1 + dz1 * dt;
+        dtheta1 = dz1;
         
-        pulses0 = curPulses;
+        //calculate dtheta2 
+        dz2 = - lambda * z2 + lambda * theta2;
+        z2 = z2 + dz2 * dt;
+        dtheta2 = dz2;
+         
         t0 = t;
         Thread::wait(1);
     }
@@ -62,9 +97,13 @@
  
     wait(2.0);
  
-    for (int i=0; i < index; i=i+2) {
- 
-        fprintf(fp,"%f,%f\n", buffer[i],buffer[i+1]);
+    for (int i=0; i < index; i=i+DATA_COLS) 
+    {
+        for (int j = 0; j< DATA_COLS; j++)
+        {
+            fprintf(fp,"%f,", buffer[i+j]);
+        }
+        fprintf(fp,"\n");
     }
     pc.printf("closing file\n\r");
     fclose(fp);
@@ -73,7 +112,9 @@
 
 int main() {
     //allocate memory for the buffer
-    buffer = new float[NR_SAMPLES];
+    pc.printf("creating buffer!\r\n");
+    buffer = new float[buffer_size];
+    pc.printf("done creating buffer!\r\n");
     T.start();
     Thread thrd2(computing,NULL,osPriorityRealtime);
     Thread thrd3(saving,NULL,osPriorityNormal);