Diff: main.cpp

Revision:

12:264e942f904f

Parent:

10:6829abb438fc

--- a/main.cpp	Tue May 17 09:58:58 2016 +0000
+++ b/main.cpp	Wed Jun 15 05:24:52 2016 +0000
@@ -4,20 +4,24 @@
 #include "FastPWM.h"
 #include "Transforms.h"
 #include "config.h"
+#include "filters.h"
 
 FastPWM *a;
 FastPWM *b;
 FastPWM *c;
 DigitalOut en(EN);
 DigitalOut toggle(PC_10);
+AnalogIn pot1(PC_5);
+AnalogIn pot2(PC_4);
 
-PositionSensorEncoder pos(CPR, 0);
+
+//PositionSensorEncoder pos(CPR, 0);
 
 Serial pc(USBTX, USBRX);
 
 int state = 0;
 int adval1, adval2;
-float ia, ib, ic, alpha, beta, d, q, vd, vq, p;
+float ia, ib, ic, alpha, beta, d, q, vd, vq, p, flux_cmd, speed_cmd, flux_cmd_raw, speed_cmd_raw, volt_cmd;
 
 float ia_supp_offset = 0.0f, ib_supp_offset = 0.0f; //current sensor offset due to bias resistor inaccuracies, etc (mV)
 
@@ -25,6 +29,9 @@
 float last_d = 0.0f, last_q = 0.0f;
 float d_ref = -0.0f, q_ref = -50.0f;
 
+MeanFilter filter_speed_cmd(0.999f);
+MeanFilter filter_flux_cmd(0.999f);
+
 void commutate();
 void zero_current();
 void config_globals();
@@ -129,34 +136,75 @@
 }
 
 void startup_msg() {
-    pc.printf("%s\n\r\n\r", "FOC'ed in the Bot Rev A.");
-    pc.printf("%s\n\r", "====Config Data====");
-    pc.printf("Current Sensor Offset: %f mV\n\r", I_OFFSET);
-    pc.printf("Current Sensor Scale: %f mv/A\n\r", I_SCALE);
-    pc.printf("Bus Voltage: %f V\n\r", BUS_VOLTAGE);
-    pc.printf("Pole pairs: %d\n\r", (int) POLE_PAIRS);
-    pc.printf("Resolver lobes: %d\n\r", (int) RESOLVER_LOBES);
-    pc.printf("Loop KP: %f\n\r", KP);
-    pc.printf("Loop KI: %f\n\r", KI);
-    pc.printf("Ia offset: %f mV\n\r", ia_supp_offset);
-    pc.printf("Ib offset: %f mV\n\r", ib_supp_offset);
+    pc.printf("%s\n\r\n\r", "Serial Begin");
     pc.printf("\n\r");
 }    
 
 void commutate() {
-    p = pos.GetElecPosition() - POS_OFFSET;
+    //p = pos.GetElecPosition() - POS_OFFSET;
+    p+=1.0f*speed_cmd; //top speed 800 electrical hz
+    
     if (p < 0) p += 2 * PI;
     
     float sin_p = sinf(p);
     float cos_p = cosf(p);
+    //DAC->DHR12R2 = (unsigned int) (speed_cmd * 4096);
     
-    float pos_dac = 0.85f * p / (2 * PI) + 0.05f;
-    DAC->DHR12R2 = (unsigned int) (pos_dac * 4096);
+    DAC->DHR12R2 = (unsigned int) ((((sin_p + 1.0f)/2.0f)*volt_cmd) * 3000);
     
     ia = ((float) adval1 / 4096.0f * AVDD - I_OFFSET - ia_supp_offset) / I_SCALE;
     ib = ((float) adval2 / 4096.0f * AVDD - I_OFFSET - ib_supp_offset) / I_SCALE;
     ic = -ia - ib;
     
+    vd = volt_cmd;
+    vq = 0.0f;
+    
+    if (vd < -1.0f) vd = -1.0f;
+    if (vd > 1.0f) vd = 1.0f;
+    if (vq < -1.0f) vq = -1.0f;
+    if (vq > 1.0f) vq = 1.0f;
+    
+    float valpha = vd * cos_p - vq * sin_p;
+    float vbeta = vd * sin_p + vq * cos_p;
+    
+    float va = valpha;
+    float vb = -0.5f * valpha - sqrtf(3) / 2.0f * vbeta;
+    float vc = -0.5f * valpha + sqrtf(3) / 2.0f * vbeta;
+    
+    set_dtc(a, 0.5f + 0.5f * va);
+    set_dtc(b, 0.5f + 0.5f * vb);
+    set_dtc(c, 0.5f + 0.5f * vc);
+}
+    
+int main() {
+    config_globals();
+    startup_msg();
+    
+    for (;;) {
+        flux_cmd_raw = pot1.read();
+        speed_cmd_raw = pot2.read();
+        speed_cmd = filter_speed_cmd.Update(speed_cmd_raw);
+        flux_cmd = filter_flux_cmd.Update(flux_cmd_raw);
+        
+        volt_cmd = flux_cmd * speed_cmd;  // both values range from 0-1.  speed corresponds to 0-x, where x is defined at the top of commutate().  
+        //increase the coefficient of flux_cmd and cap volt_cmd from 0-1 to adjust location of the transition to constant voltage:frequency
+        volt_cmd *= 2.0f;
+        if(volt_cmd > 1.0f) {volt_cmd = 1.0f;}
+        
+        
+        //pc.printf("%f\t%f\r\n", flux_cmd, speed_cmd);
+        //wait_ms(50);
+    }
+}
+
+/*
+if commutate() runs at 5kHz, 
+assume a 4 pole motor running at 6000RPM.  100RPS mechanical.  400Hz electrical.  speed_cmd(0:1) should map from 0 to 400Hz.  
+since commutate() runs at 5kHz, position+= x*speed_cmd(0:1)
+
+in a second, position should be 400*2pi.  the increment per loop is 400*2pi/5000  8*pi/50 or 4*pi/25, or about 0.5 
+*/
+    /*
     float u = ia;
     float v = ib;
     
@@ -179,36 +227,4 @@
     
     vd = KP * d_err + d_integral;
     vq = KP * q_err + q_integral;
-    
-    if (vd < -1.0f) vd = -1.0f;
-    if (vd > 1.0f) vd = 1.0f;
-    if (vq < -1.0f) vq = -1.0f;
-    if (vq > 1.0f) vq = 1.0f;
-    
-    //DAC->DHR12R2 = (unsigned int) (-q * 20 + 2048);
-    //DAC->DHR12R2 = (unsigned int) (-vd * 2000 + 2048);
-    
-    //vd = 0.0f;
-    //vq = -1.0f;
-    
-    float valpha = vd * cos_p - vq * sin_p;
-    float vbeta = vd * sin_p + vq * cos_p;
-    
-    float va = valpha;
-    float vb = -0.5f * valpha - sqrtf(3) / 2.0f * vbeta;
-    float vc = -0.5f * valpha + sqrtf(3) / 2.0f * vbeta;
-    
-    set_dtc(a, 0.5f + 0.5f * va);
-    set_dtc(b, 0.5f + 0.5f * vb);
-    set_dtc(c, 0.5f + 0.5f * vc);
-}
-    
-int main() {
-    config_globals();
-    startup_msg();
-    
-    for (;;) {
-        //pc.printf("%f\r\n", p);
-        //wait_ms(100);
-    }
-}
+    */
\ No newline at end of file