Revision 5:d41998e421ed, committed 2013-12-03

Comitter:

jaoramos

Date:

Tue Dec 03 16:16:14 2013 +0000

Parent:

4:8fcaff7801b0

Child:

6:16da0de99a8c

Commit message:

stable control with rattling on x2 encoding and second iteration on the gains

Changed in this revision

--- a/main.cpp	Mon Dec 02 04:38:30 2013 +0000
+++ b/main.cpp	Tue Dec 03 16:16:14 2013 +0000
@@ -9,9 +9,8 @@
 
 #define QUADRATURE_TYPE  2
 #define OUR_PI  3.141592653589793
-#define DATA_COLS 6
-#define NR_SAMPLES 1500
-#define buffer_size 3500
+#define DATA_COLS 7
+#define BUFFER_SIZE 4200
 #define MAX_VOLTAGE 3.3
 #define VOLTS_PER_AMP 0.14
 
@@ -32,14 +31,17 @@
 
 // open a file for data logger
 LocalFileSystem local("local");
-//const int  buffer_size = DATA_COLS * NR_SAMPLES;
 float theta1, theta2, dtheta1, dtheta2;
 float mCurrent = 0.0;
+float inputVoltage = 0.0;
 //int pulses0 = 0;
 //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;
+
+//Controller gains - Full-state Feedback
+//float k1 = -0.0316, k2 = 9.7076, k3 = -0.4095, k4 = 1.2340, k5 = 0.0410;
+float k1 = -0.3162, k2 = 18.278, k3 = -0.8964, k4 = 2.4441, k5 = 0.1843;
 
 float encoder_conv = 2*OUR_PI/(float(ENCODER_PPR)*float(QUADRATURE_TYPE));
 float motor_conv = 2*OUR_PI/(float(MOTOR_PPR)*float(QUADRATURE_TYPE));
@@ -51,13 +53,14 @@
 
 void saving(void const *args) {
     index = 0;
-    while (index < buffer_size) {
+    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;
+        buffer[index+5] = inputVoltage;
+        buffer[index+6] = t;
         index = index + DATA_COLS;
         Thread::wait(20);
     }
@@ -79,8 +82,7 @@
 }
 
 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 inputVoltage; 
+    float z1 = 0.0, z2 = 0.0, dz1 = 0.0, dz2 = 0.0, z3 = 0.0, dz3 = 0.0; 
     
     while (true) {
         t = T.read();
@@ -133,9 +135,9 @@
  
     wait(2.0);
  
-    for (int i=0; i < index; i=i+DATA_COLS) 
+    for (int i=0; i < index; i = i + DATA_COLS) 
     {
-        for (int j = 0; j< DATA_COLS; j++)
+        for (int j = 0; j < DATA_COLS; j++)
         {
             fprintf(fp,"%f,", buffer[i+j]);
         }
@@ -149,16 +151,16 @@
 int main() {
     //allocate memory for the buffer
     pc.printf("creating buffer!\r\n");
-    buffer = new float[buffer_size];
+    buffer = new float[BUFFER_SIZE];
     pc.printf("done creating buffer!\r\n");
     T.start();
-    Thread thrd2(computing,NULL,osPriorityRealtime);
-    Thread thrd3(saving,NULL,osPriorityNormal);
+    pwmOut.period(0.0001); 
     
-    //Run forward
-    pwmOut.period(0.0001); 
-    dOut1=1;
-    dOut2=0;
+    Thread thrd2(computing,NULL,osPriorityRealtime);
+    pc.printf("started computing thread!\r\n");
+    Thread thrd3(saving,NULL,osPriorityNormal);
+    pc.printf("started saving thread!\r\n");
+   
     
     pc.printf("Start!\r\n");
     pc.printf("Time: %f\r\n", t);