Diff: main.cpp

Revision:

4:1a53b06eeb7f

Parent:

2:5f5b229b004d

Child:

5:06381e54b94a

--- a/main.cpp	Tue Oct 08 14:57:00 2013 +0000
+++ b/main.cpp	Mon Sep 29 21:07:58 2014 +0000
@@ -2,12 +2,14 @@
 #include "encoder.h"
 #include "MODSERIAL.h"
 
-#define K_P 0.007//0.0184997836671646 //0.015
-#define K_I 0.00//.000824387821287097 //0
-#define K_D 0.01//.0972091946803081 //0.077
+#define K_P .01
+//0.007//0.0184997836671646 //0.015
+#define K_I 0.00
+//.000824387821287097 //0
+#define K_D .05
+//0.01//.0972091946803081 //0.077
 
-#define POT_AVG 30
-
+#define POT_AVG 50
 void coerce(float * in, float min, float max);
 float pid(float setpoint, float measurement);
 AnalogIn potmeter(PTC2);
@@ -26,34 +28,90 @@
     float voltage = potmeter.read()-.5;
 
     potsamples[sample_index] = voltage;
- 
+
     sample_index++;
     if(sample_index >= POT_AVG)
         sample_index = 0;
 }
 
+void bubblesort(float arr[], int size)
+{
+    int i,j;
+    float temp;
+    for ( i = 0 ; i <= (size/2)+1 ; i++ )
+    {
+        for ( j = 0 ; j <= (size/2)+1 - i ; j++ )
+        {
+            if ( arr[j] > arr[j + 1] )
+            {
+                temp = arr[j] ;
+                arr[j] = arr[j + 1] ;
+                arr[j + 1] = temp ;
+            }
+        }
+    }
+}
+
+void quicksort(float own_array[], int low, int high)
+{
+    int i = low;
+    int j = high;
+    int y = 0;
+    /* compare value */
+    int z = own_array[(low + high) / 2];
+    /* partition */
+    do {
+        /* find member above ... */
+        while(own_array[i] < z) i++;
+
+        /* find element below ... */
+        while(own_array[j] > z) j--;
+
+        if(i <= j) {
+            /* swap two elements */
+            y = own_array[i];
+            own_array[i] = own_array[j];
+            own_array[j] = y;
+            i++;
+            j--;
+        }
+    } while(i <= j);
+
+    /* recurse */
+    if(low <= j)
+        quicksort(own_array, low, j);
+
+    if(i <= high)
+        quicksort(own_array, i, high);
+}
+
 float getpotAverage(void)
 {
-   uint16_t valuecounter;
-   float sum = 0;
-   for(valuecounter = 0 ; valuecounter < POT_AVG ; valuecounter++)
-    {
-        sum += potsamples[valuecounter];
-    }
+    //float own_array[POT_AVG];
+    //memcpy(own_array, potsamples, sizeof(own_array));
+    //bubblesort(own_array, POT_AVG-1);
+    //return own_array[POT_AVG/2];
+    uint16_t valuecounter;
+    float sum = 0;
+    for(valuecounter = 0 ; valuecounter < POT_AVG ; valuecounter++)
+     {
+         sum += potsamples[valuecounter];
+     }
     return sum / (POT_AVG*1.);
-    
 }
 
-int main() {
+int main()
+{
     Encoder motor1(PTD0,PTC9);
     Ticker potaverage;
     MODSERIAL pc(USBTX,USBRX);
     PwmOut pwm_motor(PTA12);
     DigitalOut motordir(PTD3);
     potaverage.attach(potAverager,0.002);
-    pc.baud(115200);
+    pc.baud(921600);
     Ticker looptimer;
-    looptimer.attach(setlooptimerflag,0.01);   
+    looptimer.attach(setlooptimerflag,0.01);
+    pc.printf("bla");
     while(1) {
         float setpoint;
         float new_pwm;
@@ -63,10 +121,11 @@
         //new_pwm = (setpoint - motor1.getPosition())*.001;
         new_pwm = pid(setpoint, motor1.getPosition());
         coerce(&new_pwm, -1,1);
+        pc.printf("%f %f %f 0\n", setpoint/2000.+0.5, motor1.getPosition()/2000.+.5,new_pwm/2.+0.5);
         if(new_pwm > 0)
             motordir = 1;
         else
-            motordir = 0;  
+            motordir = 0;
         pwm_motor.write(abs(new_pwm));
     }
 }
@@ -77,23 +136,23 @@
 //'ternary operators'.
 void coerce(float * in, float min, float max)
 {
-    *in > min ? *in < max? : *in = max: *in = min;
+*in > min ? *in < max? : *in = max: *in = min;
 }
 
 
 float pid(float setpoint, float measurement)
 {
-        float error;
-        static float prev_error = 0;
-        float           out_p = 0;
-        static float    out_i = 0;
-        float           out_d = 0;
-        error  = setpoint-measurement;
-        out_p  = error*K_P;
-        out_i += error*K_I;
-        out_d  = (error-prev_error)*K_D;
-        coerce(&out_i,-0.5,0.5);
-        prev_error = error;
-        return out_p + out_i + out_d;    
+    float error;
+    static float prev_error = 0;
+    float           out_p = 0;
+    static float    out_i = 0;
+    float           out_d = 0;
+    error  = setpoint-measurement;
+    out_p  = error*K_P;
+    out_i += error*K_I;
+    out_d  = (error-prev_error)*K_D;
+    coerce(&out_i,-0.5,0.5);
+    prev_error = error;
+    return out_p + out_i + out_d;
 }