Revision 55:71a6e5fe5805, committed 2020-12-15

Comitter:

WinnieLiu

Date:

Tue Dec 15 07:28:27 2020 +0000

Parent:

54:59575833d16f

Commit message:

2020/12/15

Changed in this revision

--- a/CAN/CAN_com.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/CAN/CAN_com.cpp	Tue Dec 15 07:28:27 2020 +0000
@@ -1,16 +1,33 @@
 #include "CAN_com.h"
 
+/* -----
+移除速度控制，把位置控制改成PID
+
+log file:
+11/05: 改position的上下限(0~359.9999)，移除velocity command，新增ki command
+11/24: 更改can reply data packet structure(position: 2bytes -> 4bytes)
+11/26: 更改can reply data packet structure(position: 32bits -> 28bits)
+>> 因為會overflow
+-----*/
+
 
- #define P_MIN -95.5f
- #define P_MAX 95.5f
+
+ #define P_MIN 0.0f
+ #define P_MAX 6.283185f //359.9999 deg
+ #define P_REPLY_MIN -21360*2*PI
+ #define P_REPLY_MAX 21360*2*PI //rad
+ //#define P_MIN -95.5f
+ //#define P_MAX 95.5f
  #define V_MIN -45.0f
  #define V_MAX 45.0f
  #define KP_MIN 0.0f
  #define KP_MAX 500.0f
+ #define KI_MIN 0.0f
+ #define KI_MAX 10.0f
  #define KD_MIN 0.0f
  #define KD_MAX 5.0f
- #define T_MIN -18.0f
- #define T_MAX 18.0f
+ #define T_MIN -40.0f
+ #define T_MAX 40.0f
  
 
 /// CAN Reply Packet Structure ///
@@ -25,15 +42,40 @@
 /// 3: [velocity[3-0], current[11-8]]
 /// 4: [current[7-0]]
 void pack_reply(CANMessage *msg, float p, float v, float t){
-    int p_int = float_to_uint(p, P_MIN, P_MAX, 16);
+    int p_int = float_to_uint(p, P_REPLY_MIN, P_REPLY_MAX, 28);
+    //int p_int = float_to_uint(p, P_MIN, P_MAX, 16);
     int v_int = float_to_uint(v, V_MIN, V_MAX, 12);
-    int t_int = float_to_uint(t, -T_MAX, T_MAX, 12);
+    int t_int = float_to_uint(t, T_MIN, T_MAX, 12);
+    
     msg->data[0] = CAN_ID;
+    msg->data[1] = p_int >> 20;
+    msg->data[2] = (p_int>>12)&0xFF;
+    msg->data[3] = (p_int>>4)&0xFF;
+    msg->data[4] = (((p_int)&0xF)<<4)+(t_int>>8);
+    msg->data[5] = (t_int&0xFF);
+    
+    /*
+    for(int i = 1 ; i < 6 ; i++)
+        printf("%x  ",msg->data[i]);
+    printf("\n\r");
+    */
+
+    /*
+    msg->data[1] = p_int>>24;
+    msg->data[2] = (p_int>>16)&0xFF;
+    msg->data[3] = (p_int<<8)&0xFF;
+    msg->data[4] = p_int&0xFF;
+    msg->data[5] = v_int>>4;
+    msg->data[6] = ((v_int&0xF)<<4) + (t_int>>8);
+    msg->data[7] = t_int&0xFF;
+    */
+    /*
     msg->data[1] = p_int>>8;
     msg->data[2] = p_int&0xFF;
     msg->data[3] = v_int>>4;
     msg->data[4] = ((v_int&0xF)<<4) + (t_int>>8);
     msg->data[5] = t_int&0xFF;
+    */
     }
     
 /// CAN Command Packet Structure ///
@@ -54,18 +96,22 @@
 /// 7: [torque[7-0]]
 void unpack_cmd(CANMessage msg, ControllerStruct * controller){
         int p_int = (msg.data[0]<<8)|msg.data[1];
-        int v_int = (msg.data[2]<<4)|(msg.data[3]>>4);
-        int kp_int = ((msg.data[3]&0xF)<<8)|msg.data[4];
+        //int v_int = (msg.data[2]<<4)|(msg.data[3]>>4);
+        //int kp_int = ((msg.data[3]&0xF)<<8)|msg.data[4];
+        int kp_int = (msg.data[2]<<4)|(msg.data[3]>>4);
+        int ki_int = ((msg.data[3]&0xF)<<8)|msg.data[4]; 
         int kd_int = (msg.data[5]<<4)|(msg.data[6]>>4);
         int t_int = ((msg.data[6]&0xF)<<8)|msg.data[7];
         
         controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16);
-        controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 12);
+        //controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 12);
         controller->kp = uint_to_float(kp_int, KP_MIN, KP_MAX, 12);
+        controller->ki = uint_to_float(ki_int, KI_MIN, KI_MAX, 12);
         controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 12);
         controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12);
     //printf("Received   ");
+    //printf("%.3f  %.3f  %.3f  %.3f  %.3f   %.3f", controller->p_des, controller->kp, controller->ki, controller->kd, controller->t_ff, controller->i_q_ref);
     //printf("%.3f  %.3f  %.3f  %.3f  %.3f   %.3f", controller->p_des, controller->v_des, controller->kp, controller->kd, controller->t_ff, controller->i_q_ref);
     //printf("\n\r");
-    }
+}
 

--- a/FOC/foc.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/FOC/foc.cpp	Tue Dec 15 07:28:27 2020 +0000
@@ -1,5 +1,14 @@
+/* -----
+移除速度控制，把位置控制改成PID
+
+log file:
+11/05: 把現在的控制架構改成PID(torque_control)
+----- */
+
 
 #include "foc.h"
+
+//#define POS_MAX 6.283185f //359.9999 deg
 using namespace FastMath;
 
 
@@ -220,7 +229,39 @@
     
     
 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);
+    /*----- convert theta_mech to 0~359.9999deg -----*/
+    static float pos, round;
+    pos = controller->theta_mech;
+    modf(pos/(2*PI),&round);
+    pos = pos - round*2*PI;
+    if(abs(pos) <= 0.001)
+        pos = abs(pos);
+    else if(pos < 0)
+        pos = pos + 2*PI;
+    /*-----------------------------------------------*/
+    
+    /*----- position PID control -----*/
+    static float in_err = 0, err = 0; //integral of position error
+    if(controller->p_des < pos)
+        if((controller->p_des + 2*PI - pos) < (pos - controller->p_des))
+            err = 2*PI - pos + controller->p_des;
+        else
+            err = controller->p_des - pos;
+    else
+        if((pos + 2*PI - controller->p_des) < (controller->p_des - pos))
+            err = controller->p_des - 2*PI - pos;
+        else
+            err = controller->p_des - pos;
+    
+    in_err = in_err + err;
+    /*
+    err = controller->p_des - pos;
+    in_err = in_err + err;
+    */
+    float torque_ref = controller->kp*(err) + controller->t_ff + controller->ki*(in_err) + controller->kd*(-controller->dtheta_mech);
+    /*--------------------------------*/        
+    
+    //float torque_ref = controller->kp*(controller->p_des - controller->theta_mech) + controller->t_ff + controller->kd*(controller->v_des - controller->dtheta_mech);
     //float torque_ref = -.1*(controller->p_des - controller->theta_mech);
     controller->i_q_ref = torque_ref/KT_OUT;    
     controller->i_d_ref = 0.0f;

--- a/main.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/main.cpp	Tue Dec 15 07:28:27 2020 +0000
@@ -58,9 +58,13 @@
 volatile int state = REST_MODE;
 volatile int state_change;
 
+Timer t;
+
 void onMsgReceived() {
     //msgAvailable = true;
     //printf("%d\n\r", rxMsg.id);
+    //t.reset();
+    //t.start();
     can.read(rxMsg);  
     if((rxMsg.id == CAN_ID)){
         controller.timeout = 0;
@@ -74,13 +78,20 @@
             gpio.led->write(0);; 
             }
         else if(((rxMsg.data[0]==0xFF) & (rxMsg.data[1]==0xFF) & (rxMsg.data[2]==0xFF) & (rxMsg.data[3]==0xFF) * (rxMsg.data[4]==0xFF) & (rxMsg.data[5]==0xFF) & (rxMsg.data[6]==0xFF) & (rxMsg.data[7]==0xFE))){
+            //t.reset();
+            //t.start();
             spi.ZeroPosition();
+            //t.stop();
+            //printf("Set zero position time taken was %lf seconds\n", t.read());
+            
             }
         else if(state == MOTOR_MODE){
             unpack_cmd(rxMsg, &controller);
             }
         pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
         can.write(txMsg);
+        //t.stop();
+        //printf("can time taken was %lf seconds\n",t.read());
         }
     
 }
@@ -280,6 +291,7 @@
                     state_change = 1;
                     break;
                 case 'z':
+
                     spi.SetMechOffset(0);
                     spi.Sample(DT);
                     wait_us(20);
@@ -289,6 +301,7 @@
                         prefs.close();    
                         prefs.load(); 
                     spi.SetMechOffset(M_OFFSET);
+
                     printf("\n\r  Saved new zero position:  %.4f\n\r\n\r", M_OFFSET);
                     
                     break;
@@ -358,6 +371,7 @@
             
         }
     }
+
        
 int main() {
     controller.v_bus = V_BUS;
@@ -434,9 +448,7 @@
     printf(" Output Zero Position:  %.4f\n\r", M_OFFSET);
     printf(" CAN ID:  %d\n\r", CAN_ID);
     
-
-
-
+    
     //printf(" %d\n\r", drv.read_register(DCR));
     //wait_us(100);
     //printf(" %d\n\r", drv.read_register(CSACR));
@@ -453,13 +465,34 @@
     while(1) {
         drv.print_faults();
         wait(.1);
+        
+        //if(state == MOTOR_MODE)
+        //    printf("i_q*kt_out: %f \n\r",controller.i_q_filt*KT_OUT);
+        
+        //pack_reply(&txMsg, controller.theta_mech, controller.dtheta_mech, controller.i_q_filt*KT_OUT);
+        //can.write(txMsg);
        //printf("%.4f\n\r", controller.v_bus);
-       /*
+
+        /*
         if(state == MOTOR_MODE)
         {
             //printf("%.3f  %.3f  %.3f\n\r", (float)observer.temperature, (float)observer.temperature2, observer.resistance);
             //printf("%.3f  %.3f  %.3f %.3f %.3f\n\r", controller.v_d, controller.v_q, controller.i_d_filt, controller.i_q_filt, controller.dtheta_elec);
             //printf("%.3f\n\r", controller.dtheta_mech);
+            float pos, intpart;
+            pos = controller.theta_mech;
+            modf(pos/(2*PI),&intpart);
+            pos = pos - intpart*2*PI;
+            
+            if(abs(pos) <= 0.001)
+                pos = abs(pos);
+            else if(pos < 0)
+                pos = pos + 2*PI;
+            
+            //printf("intpart: %f\n\r",intpart);
+            //printf("theta_mech: %f\n\r",controller.theta_mech);
+            printf("pos: %f\n\r",pos);
+            
             wait(.002);
         }
         */

--- a/structs.h	Thu Aug 08 17:39:43 2019 +0000
+++ b/structs.h	Tue Dec 15 07:28:27 2020 +0000
@@ -4,6 +4,12 @@
 #include "mbed.h"
 #include "FastPWM.h"
 
+/* -----
+移除速度控制，把位置控制改成PID
+
+log file:
+11/05: 在ControllerStruct新增ki(desired KI)，保留v_des(desired velocity)
+----- */
 
 typedef struct{
     DigitalOut *enable;
@@ -33,7 +39,7 @@
     int timeout;                                            // Watchdog counter
     int mode;
     int ovp_flag;                                           // Over-voltage flag
-    float p_des, v_des, kp, kd, t_ff;                       // Desired position, velocity, gians, torque
+    float p_des, v_des, kp, ki, kd, t_ff;                       // Desired position, velocity, gians, torque
     float v_ref, fw_int;                                     // output voltage magnitude, field-weakening integral
     float cogging[128];
     } ControllerStruct;