Diff: CURRENT_CONTROL.cpp

Revision:

9:d8157fbfcd2a

Parent:

8:fd6fb3cb12ec

Child:

10:9f5f4a22346c

--- a/CURRENT_CONTROL.cpp	Mon Dec 19 15:27:13 2016 +0000
+++ b/CURRENT_CONTROL.cpp	Wed Dec 21 17:39:56 2016 +0000
@@ -7,6 +7,7 @@
     Ts = samplingTime;
     cutOff_freq_Hz = cutOff_freq_Hz_in;
     alpha_Ts = (2*3.1415926)*cutOff_freq_Hz*Ts;
+    One_alpha_Ts = 1.0 - alpha_Ts;
     output = 0.0;
     
     //
@@ -22,8 +23,9 @@
         return output;
     }
     
-    // output = (1.0 - alpha_Ts)*output + alpha_Ts*input;
+    // output = One_alpha_Ts*output + alpha_Ts*input;
     output += alpha_Ts*(input - output);
+    
     return output;
 }
 void LPF::reset(float input)
@@ -85,13 +87,24 @@
     // Speed
     angularSpeed = 0.0;
     Flag_SpeedCal_Iterated = false;
+    
+    // Reverse flage for each signal
+    reverse_current = false;
+    reverse_rotationalSpeed = false;
+    reverse_voltage = false;
 }
 //
-void CURRENT_CONTROL::SetParams(float Analog2Cur, float angSpeed2BackEmf, float voltage2DutyRatio)
+void CURRENT_CONTROL::SetParams(float Analog2Cur_in, float angSpeed2BackEmf, float voltage2Duty_in)
 {
-    analog2Cur = Analog2Cur;//LTS25-NP current sensor working with STM32F446RE : 3.3*8/0.6
+    analog2Cur = Analog2Cur_in;//LTS25-NP current sensor working with STM32F446RE : 3.3*8/0.6
     Ke = angSpeed2BackEmf;
-    voltage2Duty = voltage2DutyRatio; 
+    voltage2Duty = voltage2Duty_in; 
+}
+void CURRENT_CONTROL::SetReversal(bool reverse_current_in, bool reverse_rotationalSpeed_in, bool reverse_voltage_in)
+{
+    reverse_current = reverse_current_in;
+    reverse_rotationalSpeed = reverse_rotationalSpeed_in;
+    reverse_voltage = reverse_voltage_in;
 }
 //
 void CURRENT_CONTROL::SetGain(float Kp, float Ki, float Kd, float Ka)
@@ -107,13 +120,40 @@
 void CURRENT_CONTROL::OffsetInit(void){
     if (Flag_Init) return;
     //
+    
     Init_count++;
+    /*
     Accumulated_offset += GetAnalogIn();
     
     if (Init_count >= 500){ // Fixed time
         currentOffset = Accumulated_offset / float(Init_count);
         Flag_Init = true;
     }
+    */
+    
+    //
+    /*
+    if (Init_count <= 10){
+        Accumulated_offset = GetAnalogIn();
+    }else{
+        // Accumulated_offset +=  0.0063*(GetAnalogIn() - Accumulated_offset); // fc = 1 Hz
+        Accumulated_offset = 0.9937*Accumulated_offset + 0.0063*GetAnalogIn(); // fc = 1 Hz
+    }
+    
+    if (Init_count >= 300){ // Fixed time: 500 samples
+        currentOffset = Accumulated_offset;
+        Flag_Init = true;
+    }
+    */
+    
+    
+    //
+    if (Init_count >= 10){ // Fixed time: 10 samples
+        currentOffset = GetAnalogIn();
+        Flag_Init = true;
+    }
+    
+    
 }
 
 //
@@ -129,16 +169,22 @@
     }
     return input_value;
 }
-//
-
+// Speed_IterateOnce
+////////////////////////////////
 float CURRENT_CONTROL::Speed_IterateOnce(void){
     // The flag "Flag_SpeedCal_Iterated" will be resetted at the end of function Control()
     if(Flag_SpeedCal_Iterated) return; 
     
     // Speed
     wheelSpeed.Calculate();
-    angularSpeed = wheelSpeed.getAngularSpeed();
     
+    //
+    if (reverse_rotationalSpeed)
+        angularSpeed = -wheelSpeed.getAngularSpeed();
+    else
+        angularSpeed = wheelSpeed.getAngularSpeed();
+    //
+        
     // Flag
     Flag_SpeedCal_Iterated = true;
     
@@ -149,10 +195,19 @@
     return angularSpeed;
 }
 float CURRENT_CONTROL::getAngularSpeed_deg_s(void){
-    return wheelSpeed.getAngularSpeed_deg_s();
+    
+    if (reverse_rotationalSpeed)
+        return -wheelSpeed.getAngularSpeed_deg_s();
+    else
+        return wheelSpeed.getAngularSpeed_deg_s();
+    //
+    // return wheelSpeed.getAngularSpeed_deg_s();
 }
-//
-void CURRENT_CONTROL::Control(float curRef, float angularSpeed_in)
+//////////////////////////////// end Speed_IterateOnce
+
+// Control
+////////////////////////////////
+void CURRENT_CONTROL::Control(float curRef)
 {
     // Init check
     OffsetInit();
@@ -174,7 +229,7 @@
     voltage_out = -pid.output + func_back_emf(angularSpeed); 
     
     // Output saturation and unit changing
-    SetPWMDuty( 0.5 + saturation( voltage_out*voltage2Duty, delta_output, 0.5, -0.5) );
+    SetVoltage(voltage_out);
     
     
     //--------------------------------//
@@ -186,6 +241,7 @@
     // Reset flag
     Flag_SpeedCal_Iterated = false;
 }
+//////////////////////////////// end Control
 // Back emf as the function of rotational speed
 float CURRENT_CONTROL::func_back_emf(const float &W_in){
     return (Ke*W_in);
@@ -201,7 +257,15 @@
         TIM1->CCER |= 64;
     }
 }
-
+void CURRENT_CONTROL::SetVoltage(float volt)
+{
+    // Output saturation and unit changing
+    if (reverse_voltage)
+        SetPWMDuty( 0.5 - saturation( volt*voltage2Duty, delta_output, 0.5, -0.5) );
+    else
+        SetPWMDuty( 0.5 + saturation( volt*voltage2Duty, delta_output, 0.5, -0.5) );
+}
+//
 float CURRENT_CONTROL::GetAnalogIn(void)
 {
     analogInValue = currentAnalogIn.read();
@@ -214,15 +278,20 @@
     if (!Flag_Init) return 0.0; 
     
     //-----------------------------------------//
-    analogInValue = currentAnalogIn.read();
+    GetAnalogIn();
     
     // Unit transformation
-    curFeedBack = (analogInValue - currentOffset)*analog2Cur;
+    if (reverse_current){
+        curFeedBack = -1*(analogInValue - currentOffset)*analog2Cur;
+    }else{
+        curFeedBack = (analogInValue - currentOffset)*analog2Cur;
+    }
+    //
     
     // Low-pass filter
     curFeedBack_filter = lpFilter.filter(curFeedBack);
     
-    return  curFeedBack; //curFeedBack_filter;   
+    return  curFeedBack_filter; //curFeedBack;   
 }