Diff: main.cpp

Revision:

20:91ae97a811e3

Parent:

19:a6cf15f89f3d

Child:

21:b7fb355c8c2d

--- a/main.cpp	Wed Nov 02 12:52:00 2016 +0000
+++ b/main.cpp	Sat Nov 05 07:52:36 2016 +0000
@@ -101,9 +101,6 @@
     float sin_p = sinf(p);
     float cos_p = cosf(p);
     
-    //float pos_dac = 0.85f * p / (2 * PI) + 0.05f; //uncomment me to write position to the DAC
-    //DAC->DHR12R2 = (unsigned int) (pos_dac * 4096);
-    
     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;
@@ -123,29 +120,11 @@
     d_integral += d_err * KI;
     q_integral += q_err * KI;
     
-    if (q_integral > INTEGRAL_MAX) q_integral = INTEGRAL_MAX;
-    if (d_integral > INTEGRAL_MAX) d_integral = INTEGRAL_MAX;
-    if (q_integral < -INTEGRAL_MAX) q_integral = -INTEGRAL_MAX;
-    if (d_integral < -INTEGRAL_MAX) d_integral = -INTEGRAL_MAX;
+    q_integral = constrain(q_integral, -INTEGRAL_MAX, INTEGRAL_MAX);
+    d_integral = constrain(d_integral, -INTEGRAL_MAX, INTEGRAL_MAX);
     
-    if(control_enabled) {
-        vd = KP * d_err + d_integral;
-        vq = KP * q_err + q_integral;
-    } else {
-        vd = 0;
-        vq = 0;
-    }
-    
-    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); //uncomment me to write I_q to the DAC
-    //DAC->DHR12R2 = (unsigned int) (-vd * 2000 + 2048);
-    
-    //vd = 0.0f; //uncomment me for voltage mode testing
-    //vq = 1.0f;
+    vd = constrain(vd, -1.0f, 1.0f);
+    vq = constrain(vq, -1.0f, 1.0f);
     
     float valpha = vd * cos_p - vq * sin_p;
     float vbeta = vd * sin_p + vq * cos_p;
@@ -169,8 +148,6 @@
     startup_msg();
     
     for (;;) {
-        //pc.printf("%f\n\r", p);
-        //wait(0.1);
     }
 }