Diff: FOC/foc.cpp

Revision:

52:8fff6f1a3f50

Parent:

44:efcde0af8390

--- a/FOC/foc.cpp	Tue Oct 26 08:56:41 2021 +0000
+++ b/FOC/foc.cpp	Mon Dec 20 01:24:06 2021 +0000
@@ -1,8 +1,8 @@
-
+ 
 #include "foc.h"
 using namespace FastMath;
-
-
+ 
+ 
 void abc( float theta, float d, float q, float *a, float *b, float *c){
     /// Inverse DQ0 Transform ///
     ///Phase current amplitude = lengh of dq vector///
@@ -39,7 +39,7 @@
     *dtc_w = fminf(fmaxf(((w -v_offset)/v_bus + .5f), DTC_MIN), DTC_MAX);  
     
     }
-
+ 
 void zero_current(int *offset_1, int *offset_2){                                // Measure zero-offset of the current sensors
     int adc1_offset = 0;
     int adc2_offset = 0;
@@ -56,7 +56,7 @@
     *offset_1 = adc1_offset/n;
     *offset_2 = adc2_offset/n;
     }
-
+ 
 void reset_foc(ControllerStruct *controller){
     TIM1->CCR3 = (PWM_ARR>>1)*(0.5f);
     TIM1->CCR1 = (PWM_ARR>>1)*(0.5f);
@@ -71,8 +71,8 @@
     controller->v_q = 0;
     controller->v_d = 0;
     }
-
-
+ 
+ 
 void commutate(ControllerStruct *controller, ObserverStruct *observer, GPIOStruct *gpio, float theta){
        /// Observer Prediction ///
        observer->i_d_est += DT*(observer->i_d_dot);
@@ -143,7 +143,7 @@
         //controller->v_v = (0.86602540378f*s-.5f*c)*controller->v_d - (-0.86602540378f*c-.5f*s)*controller->v_q;
         //controller->v_w = (-0.86602540378f*s-.5f*c)*controller->v_d - (0.86602540378f*c-.5f*s)*controller->v_q;
        svm(controller->v_bus, controller->v_u, controller->v_v, controller->v_w, &controller->dtc_u, &controller->dtc_v, &controller->dtc_w); //space vector modulation
-
+ 
        observer->i_d_dot = 0.5f*(controller->v_d - 2.0f*(observer->i_d_est*R_PHASE - controller->dtheta_elec*L_Q*observer->i_q_est))/L_D;   //feed-forward voltage
        observer->i_q_dot =  0.5f*(controller->v_q - 2.0f*(observer->i_q_est*R_PHASE  + controller->dtheta_elec*(L_D*observer->i_d_est + WB)))/L_Q;
        
@@ -157,10 +157,10 @@
             TIM1->CCR1 = (PWM_ARR)*(1.0f-controller->dtc_v);
             TIM1->CCR2 =  (PWM_ARR)*(1.0f-controller->dtc_w);
         }
-
+ 
        controller->theta_elec = theta;                                          //For some reason putting this at the front breaks thins
        
-
+ 
        if(controller->loop_count >400){
            //controller->i_q_ref = -controller->i_q_ref;
           controller->loop_count  = 0;
@@ -180,10 +180,93 @@
     controller->i_q_ref = torque_ref/KT_OUT;    
     controller->i_d_ref = 0.0f;
     }
-
-
+ 
+ 
 /*    
 void zero_encoder(ControllerStruct *controller, GPIOStruct *gpio, ){
     
     }
-*/    
\ No newline at end of file
+*/    
+/**********************************WYC 2021.10.22*****************************************/
+float PID_operator (ControllerStruct *controller){
+    //printf("piding");
+    // calculate the time from the last call
+    unsigned long timestamp_now = controller->loop_count; //check the loop_count
+    float Ts = (timestamp_now - controller->timestamp_prev) * DT;
+    //printf("ts:%f\n",Ts);
+
+    // quick fix for strange cases (micros overflow)
+    if(Ts <= 0 || Ts > 0.5f) Ts = 0.001;
+
+    // u(s) = (P + I/s + Ds)e(s)
+    // Discrete implementations
+    // proportional part
+    // u_p  = P *e(k)
+    float error = controller->error;
+    float proportional = controller->kp * error;
+    // Tustin transform of the integral part
+    // u_ik = u_ik_1  + I*Ts/2*(ek + ek_1)
+    float integral = controller->integral_prev + controller->ki*Ts*0.5f*(error + controller->error_prev);
+    // antiwindup - limit the output
+    limit(&integral, -(controller->LIMIT), controller->LIMIT);
+    // Discrete derivation
+    // u_dk = D(ek - ek_1)/Ts
+    float derivative = controller->kd*(error - controller->error_prev)/Ts;
+
+    // sum all the components
+    float output = proportional + integral + derivative;
+    // antiwindup - limit the output variable
+
+    //limit(&output, -(controller->LIMIT), controller->LIMIT);
+
+/*
+    // if output ramp defined
+    if(output_ramp > 0){
+        // limit the acceleration by ramping the output
+        float output_rate = (output - output_prev)/Ts;
+        if (output_rate > output_ramp)
+            output = output_prev + output_ramp*Ts;
+        else if (output_rate < -output_ramp)
+            output = output_prev - output_ramp*Ts;
+    }
+    */
+    // saving for the next pass
+    controller->integral_prev = integral;
+    controller->output_prev = output;
+    controller->error_prev = error;
+    controller->timestamp_prev = timestamp_now;
+    return output;
+}
+/*********WYC ADD 2021.10.22**********/
+
+void velocity_control(ControllerStruct *controller){
+
+    controller->ki = controller->kd;
+    controller->kd = 0;
+    controller->error = (controller->v_des - controller->dtheta_mech);
+
+    float current_sp = PID_operator (controller);
+    controller->i_q_ref = current_sp;  //wyc 2021.07.26 compared with the program in mbed,old motor with no "-"in the front of "torque_des"
+    //printf("iq:%f\n",controller->i_q_des);
+    controller->i_d_ref = 0.0f;
+    
+}
+/*********WYC ADD 2021.10.22**********/  
+
+
+
+/*********YZ ADD 2021.10.22**********/
+
+void Position_control(ControllerStruct *controller){
+
+    controller->ki = controller->kd;
+    controller->kd = 0;
+    controller->error = (controller->p_des - controller->theta_mech);
+
+    float current_Position = PID_operator (controller);
+    controller->i_q_ref=current_Position;  //YZ 2021.07.26 compared with the program in mbed,old motor with no "-"in the front of "torque_des"
+    //printf("iq:%f\n",controller->i_q_des);
+    controller->i_d_ref = 0.0f;
+    
+}
+/*********YZ ADD 2021.10.22**********/      
\ No newline at end of file