Diff: main.cpp

Revision:

3:df56bf381572

Parent:

2:a7100c183940

Child:

4:f8a45966e63b

--- a/main.cpp	Fri Feb 03 20:52:05 2017 +0000
+++ b/main.cpp	Sat Feb 04 16:33:45 2017 +0000
@@ -2,22 +2,20 @@
 
 #include "mbed.h"
 #include "time.h"
+#include "stdio.h"
+#include "ctype.h"
 
 InterruptIn button(USER_BUTTON);
 PwmOut pwm(D5);//do not use D3
 DigitalOut a(D2);
 DigitalOut b(D4);
 
-int endcount, startcount;
-double time_between_readings;
-double velocity;
+AnalogIn motorVoltage(A4); //hook up to high voltage motor terminal with capacitor. this doesn't share a common ground though hmm... can I hook up battery ground
+AnalogIn currentSense(A5); //hook up to Vout on current sensor
+Serial pc(USBTX, USBRX, 115200);
+DigitalOut green(LED2);
 
-int cpr = 900;  // Encoder counts per revolution.  Change to match your encoder 
-
-AnalogIn motorVoltage(A4); //hook up to high voltage motor terminal with capacitor. this doesn't share a common ground though hmm... can I hook up battery ground
-AnalogIn motorCurrent(A5); //hook up to Vout on current sensor
-Serial pc(USBTX, USBRX);
-DigitalOut green(LED2);
+const int cpr = 900;  // Encoder counts per revolution.  Change to match your encoder 
 
 
 void EncoderInitialise(void) {
@@ -60,16 +58,16 @@
     int pwmV = (int)(100*pwm_float);
     printf("pwm2: %d\n\r",pwmV);
     if(pwmV == 0){
-        pwm.write(0.1);
-    } else if (pwmV == 10){
+        pwm.write(0.05);
+    } else if (pwmV == 5){
         pwm.write(0.2);
     } else if (pwmV == 20){
-        pwm.write(0.5);
-    } else if (pwmV == 50){
-        pwm.write(1.0);
-    } else if (pwmV == 100){
+        pwm.write(0.75);
+    } else if (pwmV == 75){
         pwm.write(0.0);
-    }         
+    } else {
+        pwm.write(0.0);
+    }
 }
 
 
@@ -78,6 +76,10 @@
     
     
     const double vRef = 3;
+    int endcount, startcount;
+    double time_between_readings;
+    double velocity;
+    double currentSensed = 0;
     clock_t start;
     clock_t end = clock();
     int ticks;
@@ -88,8 +90,9 @@
     double samplesPerPublish = (int)(publishFrequency/updatePeriod); /*this improves time response of filter and maintains smoothness*/
     int publishCounter = 1;
     double filterRatio = 0.05;
+    double currentFilterRatio = 0.02;
     float currentSensorOffset = 0; int i; 
-    for(i=1;i<101;i++){ currentSensorOffset += motorCurrent.read(); }
+    for(i=1;i<101;i++){ currentSensorOffset += currentSense.read(); }
     currentSensorOffset = currentSensorOffset*vRef/100;
     
     EncoderInitialise();
@@ -108,16 +111,24 @@
         if(abs(ticks)>cpr/2) /***** for rollover case: *****/
         { ticks = ((ticks<0)-(ticks>0))*(cpr-abs(ticks)); }
         velocity = filterRatio*((double)ticks)/cpr/time_between_readings + (1-filterRatio)*velocity; /* with filtering*/
+        
+        currentSensed = currentFilterRatio*((double)currentSense.read()*vRef-currentSensorOffset) + (1-currentFilterRatio)*currentSensed;
+        
         if(pc.readable())
+        {
+            char charIn = pc.getc();
+            if(isdigit(charIn)){
+                double abrahamsCommand = (double)(charIn - '0');
+                pwm.write(abrahamsCommand/10.0);
+            }
+        }
+                    
+        if(publishCounter == samplesPerPublish)
             {
-            char abrahamsCommand = pc.getc();
-            if(abrahamsCommand == '1'){ green = 1; }
-            if(abrahamsCommand == '0'){ green = 0; }
+//            printf("%f,%f,%f\n", (double)currentSense.read()*vRef-currentSensorOffset, motorVoltage.read(), velocity);
+            printf("%f,%f,%f\n", currentSensed, motorVoltage.read(), velocity);
+            publishCounter = 1;
             }
-        if(publishCounter == samplesPerPublish){
-            printf("%f,%f,%f\n", (double)motorCurrent.read()*vRef-currentSensorOffset, motorVoltage.read(), velocity);
-            publishCounter = 1;
-        }
         publishCounter++;
         
     }