Diff: CAN/CAN_com.cpp

Revision:

55:71a6e5fe5805

Parent:

54:59575833d16f

diff -r 59575833d16f -r 71a6e5fe5805 CAN/CAN_com.cpp
--- 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");
-    }
+}