Diff: main.cpp

Revision:

1:f97adef77f4b

Parent:

0:dc5c88c2dd20

Child:

2:a7100c183940

--- a/main.cpp	Fri Feb 03 18:18:22 2017 +0000
+++ b/main.cpp	Fri Feb 03 20:09:00 2017 +0000
@@ -76,7 +76,8 @@
 
 int main() {
     
-    EncoderInitialise();
+    
+    const double vRef = 3.3;
     clock_t start;
     clock_t end = clock();
     int ticks;
@@ -84,10 +85,17 @@
     button.fall(&pressed);
     double updatePeriod = 0.01; /* must select carefully */
     double publishFrequency = 0.1; /* seconds. rate to publish to matlab */
-    double samplesPerPublish = (int)(publishFrequency/updatePeriod); /*sample and average then publish for filtering*/
+    double samplesPerPublish = (int)(publishFrequency/updatePeriod); /*this improves time response of filter and maintains smoothness*/
     int publishCounter = 1;
-    double filterRatio = 0.3;
-
+    double filterRatio = 0.05;
+    float currentSensorOffset = 0; int i; 
+    for(i=1;i<101;i++){ currentSensorOffset += motorCurrent.read(); }
+    currentSensorOffset = currentSensorOffset*vRef/100;
+    
+    EncoderInitialise();
+    fflush(pc);
+    
+    
     while(1) {
         
         wait(updatePeriod);
@@ -106,15 +114,11 @@
             if(abrahamsCommand == '1'){ green = 1; }
             if(abrahamsCommand == '0'){ green = 0; }
             }
-            
-        printf("%f,%f,%f\n", motorCurrent.read(), motorVoltage.read(), velocity);
-            
-//        ---averaging filter---    
-        //if(publishCounter == samplesPerPublish){
-//            printf("%f,%f,%f\n", motorCurrent.read(), motorVoltage.read(), velocity);
-//            publishCounter = 1;
-//        }
-//        publishCounter++;
+        if(publishCounter == samplesPerPublish){
+            printf("%f,%f,%f\n", motorCurrent.read()*vRef-(float)currentSensorOffset, motorVoltage.read(), velocity);
+            publishCounter = 1;
+        }
+        publishCounter++;
         
     }