Revision 6:4960629abb90, committed 2015-02-26

Comitter:

nki

Date:

Thu Feb 26 14:09:19 2015 +0000

Parent:

5:eeb8af99cb6c

Commit message:

LOLOL PROPROTIONAL CURENT CONTROL;

Changed in this revision

--- a/constants.h	Thu Feb 26 09:50:36 2015 +0000
+++ b/constants.h	Thu Feb 26 14:09:19 2015 +0000
@@ -23,6 +23,12 @@
 #define THROTTLE_DB .182f
 
 #define SPEED_LPF 0.9f
+#define CURRENT_LPF 0.9f
+
+#define IMAX 0.001f //LOL ARBITRARY CURRENT UNITS
+#define Kp_i 1000.0f
+#define Ki_i 1.0f
+#define Kd_i 1.0f
 
 #define _PH_A D6
 #define _PH_B D13
@@ -41,8 +47,6 @@
 #define _IA A1
 #define _IB A2
 
-#define ISET 0
-
 #define ATAN_TABLE_SIZE 2000
 #define ATAN_UPPER_BOUND 20
 /*************************************************/
@@ -116,12 +120,17 @@
   float dtc_a, dtc_b, dtc_c;
   
   float throttle;
+  float command;
   float sensor_phase;
   float angle;
   float lastangle;
   float speed; 
   float current;
-  float izero;
+  float iazero;
+  float ibzero;
+  float ilast;
+  float iP, iI, iD;
+  float dtc;
   unsigned char debug_stop;
 } Motor;
 

--- 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

--- a/main.cpp	Thu Feb 26 09:50:36 2015 +0000
+++ b/main.cpp	Thu Feb 26 14:09:19 2015 +0000
@@ -60,7 +60,11 @@
         ia_read = ia;
         ib_read = ib;
         isense_update();
+        pc.printf("%f",motor->command);
+        pc.printf("\t");
         pc.printf("%f",motor->current);
+        pc.printf("\t");
+        pc.printf("%f",motor->dtc);
         pc.printf("\n\r");
         pos_update();
 #ifndef __USE_THROTTLE