Diff: isr.cpp

Revision:

6:4960629abb90

Parent:

5:eeb8af99cb6c

--- a/isr.cpp	Thu Feb 26 09:50:36 2015 +0000
+++ b/isr.cpp	Thu Feb 26 14:09:19 2015 +0000
@@ -15,9 +15,9 @@
     float ic = ia + 120.0f;
     if (ic >= 360.0f) ic -= 360.0f;
       
-    motor->dtc_a = sinetab[(int) (ia)] * motor->throttle;
-    motor->dtc_b = sinetab[(int) (ib)] * motor->throttle;
-    motor->dtc_c = sinetab[(int) (ic)] * motor->throttle;
+    motor->dtc_a = sinetab[(int) (ia)] * motor->dtc;
+    motor->dtc_b = sinetab[(int) (ib)] * motor->dtc;
+    motor->dtc_c = sinetab[(int) (ic)] * motor->dtc;
 #else
     float ix = 120.0f - motor->angle;
     if (ix < 0) ix += 360.0f;
@@ -132,6 +132,7 @@
     throttle_raw = (throttle_raw - THROTTLE_DB) / (1.0f - THROTTLE_DB);
     if (throttle_raw < 0.0f) throttle_raw = 0.0f;
     motor->throttle = (1.0f - THROTTLE_LPF) * throttle_raw + THROTTLE_LPF * motor->throttle;
+    motor->command = motor->throttle;
     if (motor->throttle < 0.05f & abs(motor->speed) < 1.0f) {
         motor->halt = 1;
     } else {
@@ -140,17 +141,37 @@
 }
 
 void isense_update() {
+        
     float ia = ia_read;
     float ib = ib_read;
-    float irms = sqrt(ia*ia + ib*ib + (ia+ib)*(ia+ib));
+    
+    if(motor->halt == 1){ //zero the current sensors
+        motor->iazero = ia;
+        motor->ibzero = ib;
+    }
+    
+    ia = ia-motor->iazero;
+    ib = ib-motor->ibzero;
+    
+    float irms = ia*ia + ib*ib + (ia+ib)*(ia+ib);
+    
+    motor->current = (1.0f - CURRENT_LPF) * irms + CURRENT_LPF * motor->current;
+    
+    float iset = IMAX * motor->command; 
+    motor->iP = iset - motor->current;
+//    motor->iI = motor->iIlast + motor->iP;
+//    motor->iD = motor->iP - motor->ilast;
+    motor->dtc = Kp_i*motor->iP; //+ Ki_i*motor->iI + Kd_i*motor->iD;
+    
+    if(motor->dtc < 0.0f){
+        motor->dtc = 0.0f;
+    }
+    if(motor->dtc > 1.0f){
+        motor->dtc = 1.0f;
+    }
+
     
     
-    if(motor->halt == 1){ //zero the current sensor OMG THIS IS SO SCARY OMG
-        motor->izero = irms;
-    }
     
-    motor->current = irms-motor->izero;
-    
-    //float iset = motor->imax * motor->throttle
     //float error = irms - iset  
 }
\ No newline at end of file