Diff: FOC/foc.cpp

Revision:

50:3bda887997f0

Parent:

44:efcde0af8390

--- a/FOC/foc.cpp	Mon Mar 29 07:08:14 2021 +0000
+++ b/FOC/foc.cpp	Thu Nov 04 08:16:21 2021 +0000
@@ -1,7 +1,14 @@
 
 #include "foc.h"
 using namespace FastMath;
+float pre_p_des1;
 
+float v1_error;
+float v_mech_error;
+
+float pre_v_des1;
+float pre_dtheta_mech1;
+float torque_ref;
 
 void abc( float theta, float d, float q, float *a, float *b, float *c){
     /// Inverse DQ0 Transform ///
@@ -70,6 +77,13 @@
     controller->d_int = 0;
     controller->v_q = 0;
     controller->v_d = 0;
+    //**************WYC ADD*******************2021/11/4************//
+    controller->integral_prev = 0;
+    controller->output_prev = 0;
+    controller->error_prev = 0;
+    controller->timestamp_prev = 0;
+    controller->error = 0;
+    //**************WYC END*******************2021/11/4************//
     }
 
 
@@ -175,15 +189,91 @@
     
     
 void torque_control(ControllerStruct *controller){
-    float torque_ref = controller->kp*(controller->p_des - controller->theta_mech) + controller->t_ff + controller->kd*(controller->v_des - controller->dtheta_mech);
+    v1_error = controller->v_des - pre_p_des1;
+    v_mech_error = controller->dtheta_mech - pre_dtheta_mech1;
+    //if(controller->p_des !=pre_p_des1){
+        torque_ref = controller->kp*(controller->p_des - controller->theta_mech) + controller->t_ff + controller->kd*(controller->v_des - controller->dtheta_mech);
+    //}
+    //else{
+     //   torque_ref = controller->kp*(controller->v_des - controller->dtheta_mech) +controller->kd*(v1_error - v_mech_error);
+     //   }
     //float torque_ref = -.1*(controller->p_des - controller->theta_mech);
     controller->i_q_ref = torque_ref/KT_OUT;    
     controller->i_d_ref = 0.0f;
-    }
+    
+    pre_p_des1 = controller->p_des;
+    pre_v_des1 = controller->v_des;
+    pre_dtheta_mech1 = controller->dtheta_mech;
+        }
 
 
 /*    
 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**********/
\ No newline at end of file