Diff: loopdriver.cpp

Revision:

9:d3b70c15baa9

Parent:

6:99ee0ce47fb2

Child:

10:b4abecccec7a

--- a/loopdriver.cpp	Wed Mar 04 15:33:32 2015 +0000
+++ b/loopdriver.cpp	Fri Mar 06 19:12:53 2015 +0000
@@ -20,7 +20,7 @@
     _user = user;
     _pid_d = pid_d;
     _pid_q = pid_q;
-    _reference = new SynchronousReferenceSynthesizer(max_phase_current);;
+    _reference = new SynchronousReferenceSynthesizer(max_phase_current);
     _modulator = modulator;
     _max_phase_current = max_phase_current;
     _update_frequency = update_frequency;
@@ -31,26 +31,43 @@
 }
 
 void LoopDriver::Clarke(float c, float b, float *alpha, float *beta) {
-    *alpha = b;
-    //*beta = 1.0f / sqrt(3.0f) * a + 2.0f / sqrt(3.0f) * b;
-    *beta = (b + 2.0f * c)/sqrt(3.0f);
+    /*    
+    //ignoring current for now.  These are motor position vectors (A,B,C) but no C because it's not necessary for the calculation)
+    //these numbers range from -1:1, but when we use current instead of angle, the units will be amps.  The amps will be unbounded ;)
+    float A, B; 
+    A = FastSin(_motor->angle);
+    B = FastSin(_motor->angle - 120.0f);
+    
+    //\alpha \beta (Clarke) transform
+    *alpha = A;
+    *beta = (A + 2.0f*B)/sqrt(3.0f);
+    */
+    //\alpha \beta (Clarke) transform
+    float ia = -(b+c);
+    float ib = b;
+    float ic = c;
+    
+    *alpha = ia;
+    *beta = (ia + 2.0f*ib)/sqrt(3.0f);
 }
 
 void LoopDriver::Parke(float alpha, float beta, float theta, float *d, float *q) {
-    float cos = LutCos(theta);
-    float sin = LutSin(theta);
-    *d = alpha * cos - beta * sin;
-    *q = -alpha * sin - beta * cos;
+    float cos = FastCos(theta);
+    float sin = FastSin(theta);
+    *d = alpha * cos + beta * sin;
+    *q = -alpha * sin + beta * cos;
 }
 
 void LoopDriver::InverseParke(float d, float q, float theta, float *alpha, float *beta) {
-    float cos = LutCos(theta);
-    float sin = LutSin(theta);
-    //*alpha = cos * d - sin * q;
-    //*beta = -(sin * d + cos * q);  
+    float cos = FastCos(theta);
+    float sin = FastSin(theta);
+    *alpha = cos * d - sin * q;
+    *beta = sin * d + cos * q;  
     
+    /*
     *alpha = cos * 1.0f;
-    *beta = -sin * 1.0f;                                    
+    *beta = -sin * 1.0f;  
+    */                                  
 }
 
 float LoopDriver::LutSin(float theta) {
@@ -64,14 +81,36 @@
 }
 
 void LoopDriver::update() {    
+    
     float alpha, beta, d, q, ref_d, ref_q, valpha, vbeta;
     Clarke(_motor->I_c, _motor->I_b, &alpha, &beta);
-    test_alpha = alpha;
-    test_beta = beta;
     Parke(alpha, beta, _motor->angle, &d, &q);
-    _reference->GetReference(_motor->angle, &ref_d, &ref_q);
-    float vd = _pid_d->Update(ref_d, d);
-    float vq = _pid_q->Update(ref_q, q);
+    
+    d_mean = 0.01f*d + 0.99f*d_mean;
+    q_mean = 0.01f*q + 0.99f*q_mean;
+
+    _reference->GetReference(_motor->angle, &ref_d, &ref_q);  //this just returns d = 0 and q = 1 right now.  doesn't care about angle
+    float vd = _pid_d->Update(ref_d, d_mean);
+    float vq = _pid_q->Update(ref_q, q_mean);
+    
+    test_alpha = vd;
+    test_beta = vq;
+    
+    /*
+    //overriding PI controller here because we're not actually doing current control yet
+    //These values are approximately what I saw coming out of the park transform.  just feeding them back in here to test the loop
+    vd = 0.0f;
+    vq = -1.0f;  
+    */
+    
     InverseParke(vd, vq, _motor->angle, &valpha, &vbeta);
-    _modulator->Update(valpha, vbeta);   
+
+    _modulator->Update(valpha, vbeta); 
+    
+    /*
+    _inverter->SetDtcA((LutSin(_motor->angle)/2.0f)+0.5f);
+    _inverter->SetDtcB((LutSin(_motor->angle - 120.0f)/2.0f)+0.5f);
+    _inverter->SetDtcC((LutSin(_motor->angle + 120.0f)/2.0f)+0.5f);
+    */
+    
 }