hobbyking_cheetah source code modified 2020/12/15

Dependencies:   mbed-dev-f303 FastPWM3

Files at this revision

API Documentation at this revision

Comitter:
WinnieLiu
Date:
Tue Dec 15 07:28:27 2020 +0000
Parent:
54:59575833d16f
Commit message:
2020/12/15

Changed in this revision

CAN/CAN_com.cpp Show annotated file Show diff for this revision Revisions of this file
FOC/foc.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
structs.h Show annotated file Show diff for this revision Revisions of this file
--- 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;