Diff: main.cpp

Revision:

4:f8a45966e63b

Parent:

3:df56bf381572

Child:

5:f4c237d0bb32

--- a/main.cpp	Sat Feb 04 16:33:45 2017 +0000
+++ b/main.cpp	Sat Feb 04 21:19:06 2017 +0000
@@ -5,18 +5,20 @@
 #include "stdio.h"
 #include "ctype.h"
 
+#define PI 3.14159265358979323846
+
 InterruptIn button(USER_BUTTON);
 PwmOut pwm(D5);//do not use D3
 DigitalOut a(D2);
 DigitalOut b(D4);
 
-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);
 
-const int cpr = 900;  // Encoder counts per revolution.  Change to match your encoder 
-
+const int CPR = 900;  // Encoder counts per revolution.  Change to match your encoder 
+const double VREF = 3; //Microcontroller reference voltage
+const float currentSensorOutputRatio = 0.185; // Volts/Amp. Divide Voltage by cSenseOutput to get current
 
 void EncoderInitialise(void) {
     // configure GPIO PA0 & PA1 as inputs for Encoder
@@ -38,7 +40,7 @@
     TIM2->CCMR2 = 0x0000;       //                             < TIM capture/compare mode register 2
     TIM2->CCER  = 0x0011;       // CC1P CC2P                   < TIM capture/compare enable register
     TIM2->PSC   = 0x0000;       // Prescaler = (0+1)           < TIM prescaler
-    TIM2->ARR   = cpr;          // reload at cpr               < TIM auto-reload register
+    TIM2->ARR   = CPR;          // reload at CPR               < TIM auto-reload register
     TIM2->CNT = 0x0000;         //reset the counter before we use it  
 }
 
@@ -56,7 +58,6 @@
 void pressed() {
     float pwm_float = pwm.read();
     int pwmV = (int)(100*pwm_float);
-    printf("pwm2: %d\n\r",pwmV);
     if(pwmV == 0){
         pwm.write(0.05);
     } else if (pwmV == 5){
@@ -74,8 +75,6 @@
 
 int main() {
     
-    
-    const double vRef = 3;
     int endcount, startcount;
     double time_between_readings;
     double velocity;
@@ -86,18 +85,19 @@
     a=1; b=0; pwm.write(0);
     button.fall(&pressed);
     double updatePeriod = 0.01; /* must select carefully */
-    double publishFrequency = 0.1; /* seconds. rate to publish to matlab */
+    double publishFrequency = 0.05; /* seconds. rate to publish to matlab */
     double samplesPerPublish = (int)(publishFrequency/updatePeriod); /*this improves time response of filter and maintains smoothness*/
     int publishCounter = 1;
-    double filterRatio = 0.05;
+    double filterRatio = 0.045;
     double currentFilterRatio = 0.02;
     float currentSensorOffset = 0; int i; 
-    for(i=1;i<101;i++){ currentSensorOffset += currentSense.read(); }
-    currentSensorOffset = currentSensorOffset*vRef/100;
+    for(i=1;i<301;i++){ currentSensorOffset += currentSense.read(); }
+    currentSensorOffset = currentSensorOffset*VREF/300;
     
     EncoderInitialise();
     fflush(pc);
     
+    /*** wait here for matlab information like voltage before starting? ***/
     
     while(1) {
         
@@ -108,16 +108,19 @@
         startcount = endcount;
         endcount = GetCounts();       
         ticks = endcount-startcount;
-        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*/
+        if(abs(ticks)>CPR/2) /***** for rollover case: *****/
+        { ticks = ((ticks<0)-(ticks>0))*(CPR-abs(ticks)); }
+        velocity = filterRatio*((double)ticks)/CPR*2*PI/time_between_readings + (1-filterRatio)*velocity; /* with filtering*/
         
-        currentSensed = currentFilterRatio*((double)currentSense.read()*vRef-currentSensorOffset) + (1-currentFilterRatio)*currentSensed;
+        currentSensed = currentFilterRatio*((double)currentSense.read()*VREF-currentSensorOffset) + (1-currentFilterRatio)*currentSensed;
         
         if(pc.readable())
         {
             char charIn = pc.getc();
-            if(isdigit(charIn)){
+            if(charIn == 'r'){
+                fflush(pc);
+                green = !green;
+            } else if(isdigit(charIn)) {
                 double abrahamsCommand = (double)(charIn - '0');
                 pwm.write(abrahamsCommand/10.0);
             }
@@ -125,8 +128,7 @@
                     
         if(publishCounter == samplesPerPublish)
             {
-//            printf("%f,%f,%f\n", (double)currentSense.read()*vRef-currentSensorOffset, motorVoltage.read(), velocity);
-            printf("%f,%f,%f\n", currentSensed, motorVoltage.read(), velocity);
+            printf("%f,%f,%f\n", currentSensed/currentSensorOutputRatio, pwm.read(), velocity);
             publishCounter = 1;
             }
         publishCounter++;