Diff: main.cpp

Revision:

1:5ac85aad9da4

Parent:

0:c9e647421e54

Child:

2:5f5b229b004d

--- a/main.cpp	Thu Sep 26 11:31:08 2013 +0000
+++ b/main.cpp	Thu Sep 26 13:08:21 2013 +0000
@@ -2,36 +2,66 @@
 #include "encoder.h"
 #include "MODSERIAL.h"
 
-#define K_P 0.015//184997836671646 
-#define K_I 0.//000824387821287097
-#define K_D 0.077//972091946803081
+#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 POT_AVG 30
 
 void coerce(float * in, float min, float max);
 float pid(float setpoint, float measurement);
-
+AnalogIn potmeter(PTC2);
 volatile bool looptimerflag;
+float potsamples[POT_AVG];
 
 void setlooptimerflag(void)
 {
     looptimerflag = true;
 }
 
+void potAverager(void)
+{
+
+    static uint16_t sample_index = 0;
+    float voltage = potmeter.read()-.5;
+
+    potsamples[sample_index] = voltage;
+ 
+    sample_index++;
+    if(sample_index >= POT_AVG)
+        sample_index = 0;
+}
+
+float getpotAverage(void)
+{
+   uint16_t valuecounter;
+   float sum = 0;
+   for(valuecounter = 0 ; valuecounter < POT_AVG ; valuecounter++)
+    {
+        sum += potsamples[valuecounter];
+    }
+    return sum / (POT_AVG*1.);
+    
+}
+
 int main() {
     Encoder motor1(PTD0,PTC9);
+    Ticker potaverage;
     MODSERIAL pc(USBTX,USBRX);
     PwmOut pwm_motor(PTA12);
     DigitalOut motordir(PTD3);
-    AnalogIn   potmeter(PTC2);
+    potaverage.attach(potAverager,0.002);
     pc.baud(115200);
     Ticker looptimer;
     looptimer.attach(setlooptimerflag,0.01);   
     while(1) {
-        static float setpoint = 0;
+        float setpoint;
         float new_pwm;
         while(!looptimerflag);
         looptimerflag = false;
-        setpoint = (potmeter.read()-0.5)*2000;
-        new_pwm = pid(setpoint, motor1.getPosition());
+        setpoint = (getpotAverage())*2000;
+        new_pwm = (setpoint - motor1.getPosition())*.001;
+        //new_pwm = pid(setpoint, motor1.getPosition());
         coerce(&new_pwm, -1,1);
         if(new_pwm > 0)
             motordir = 1;
@@ -41,6 +71,10 @@
     }
 }
 
+
+//coerces value 'in' to min or max when exceeding those values
+//if you'd like to understand the statement below take a google for
+//'ternary operators'.
 void coerce(float * in, float min, float max)
 {
     *in > min ? *in < max? : *in = max: *in = min;
@@ -49,7 +83,6 @@
 
 float pid(float setpoint, float measurement)
 {
-        //static int16_t setpoint = 0            //static int16_t setpoint = 0      
         float error;
         static float prev_error = 0;
         float           out_p = 0;
@@ -58,7 +91,7 @@
         error  = setpoint-measurement;
         out_p  = error*K_P;
         out_i += error*K_I;
-        out_d = (error-prev_error)*K_D;
+        out_d  = (error-prev_error)*K_D;
         coerce(&out_i,-0.5,0.5);
         prev_error = error;
         return out_p + out_i + out_d;