Diff: foc.cpp

Revision:

3:08746709f023

Parent:

2:7312ac02785d

Child:

4:4e00b310811d

--- a/foc.cpp	Sun Oct 30 19:37:46 2016 +0000
+++ b/foc.cpp	Sun Oct 30 19:55:58 2016 +0000
@@ -18,6 +18,8 @@
 float theta_e =        0.0f;                //electrical motor angle
 float ia_supp_offset = 0.0f;                //calculated current sensor offset         
 float ib_supp_offset = 0.0f;                //calculated current sensor offset
+bool control_enabled = false;               //gate drive on
+DigitalOut             en(PB_15);           //gate drive enable output
 
 
 //********MATH FUNCTIONS********/
@@ -42,12 +44,12 @@
 }
 
 //run motor control loop
-void motor_control()
+void motor_control(PWM_IN* p_in)
 {
     ADC1->CR2  |= 0x40000000; 
     volatile int delay;
     for (delay = 0; delay < 35; delay++);
-    foc(ADC1->DR, ADC2->DR);
+    foc(ADC1->DR, ADC2->DR, p_in);
 }
 
 //calculate zero current offset
@@ -67,10 +69,30 @@
 
 //*****************PRIVATE*****************//
 
-void foc(float ia, float ib)
+void go_enabled()
+{
+    d_int = 0.0f;
+    q_int = 0.0f;
+    control_enabled = true;
+    en = 1;
+}
+
+void go_disabled()
 {
+    control_enabled = false;
+    en = 0;
+}
+
+void foc(float ia, float ib, PWM_IN* p_in)
+{
+    if(control_enabled && !p_in->get_enabled()) go_disabled();
+    if(!control_enabled && p_in->get_enabled()) go_enabled();
     //get q current command from vehicle_controller and throttle value
-    q_command = velocity_control(get_throttle());
+    q_command = voltage_control(get_throttle());
+    
+    //check q current min/max
+    if(q_command > K_Q_CURRENT_MAX) q_command = K_Q_CURRENT_MAX;
+    if(q_command < K_Q_CURRENT_MIN) q_command = K_Q_CURRENT_MIN;
     
     //scale and offset a and b current values
     float i_a = ((float) ia / 4096.0f * AVDD - I_OFFSET - ia_supp_offset) / I_SCALE;