Diff: CAN/CAN_com.cpp

Revision:

55:6ab83cbbbca7

Parent:

54:59575833d16f

diff -r 59575833d16f -r 6ab83cbbbca7 CAN/CAN_com.cpp
--- a/CAN/CAN_com.cpp	Thu Aug 08 17:39:43 2019 +0000
+++ b/CAN/CAN_com.cpp	Fri Mar 26 05:28:00 2021 +0000
@@ -12,6 +12,20 @@
  #define T_MIN -18.0f
  #define T_MAX 18.0f
  
+ #define kp_L_MAX 125.0f
+ #define kp_L_MIN 0.0f
+ #define kp_M1_MAX 250.0f
+ #define kp_M1_MIN 126.0f
+ #define kp_M2_MAX 375.0f
+ #define kp_M2_MIN 251.0f
+ #define kp_H_MAX   500.0f
+ #define kp_H_MIN   376.0f
+ #define range_L_MAX 256.0f
+ #define range_L_MIN 256.0f
+ #define range_M_MAX 256.0f
+ #define range_M_MIN 256.0f
+ #define range_H_MAX 256.0f
+ #define range_H_MIN 256.0f
 
 /// CAN Reply Packet Structure ///
 /// 16 bit position, between -4*pi and 4*pi
@@ -35,7 +49,60 @@
     msg->data[4] = ((v_int&0xF)<<4) + (t_int>>8);
     msg->data[5] = t_int&0xFF;
     }
+
+/// CAN Command Packet Structure ///
+/// 16 bit position command, between -4*pi and 4*pi
+/// 12 bit velocity command, between -30 and + 30 rad/s
+/// 12 bit kp, between 0 and 500 N-m/rad
+/// 12 bit kd, between 0 and 100 N-m*s/rad
+/// 12 bit feed forward torque, between -18 and 18 N-m
+/// CAN Packet is 8 8-bit words
+/// Formatted as follows.  For each quantity, bit 0 is LSB
+/// 0: [position[15-8]]
+/// 1: [position[7-0]] 
+/// 2: [velocity[11-4]]
+/// 3: [velocity[3-0], kp[11-8]]
+/// 4: [kp[7-0]]
+/// 5: [kd[11-4]]
+/// 6: [kd[3-0], torque[11-8]]
+/// 7: [torque[7-0]]
+void unpack_cmd(CANMessage msg, ControllerStruct * controller){
+        int p_int = (msg.data[0]<<16)|(msg.data[1]<<8)|msg.data[2];
+        int v_int = (msg.data[3]<<12)|(msg.data[4]<<4)|(msg.data[5]>>4);
+        int t_int = ((msg.data[5]&0xF)<<16)|(msg.data[6]<<8)|msg.data[7];
+        
+        controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 24);
+        controller->v_des = uint_to_float(v_int, V_MIN, V_MAX, 20);
+        controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 20);
+    //printf("Received   ");
+    //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");
+    }       
+        
+void unpack_param(CANMessage msg, ControllerStruct * controller){
+                int  kp_L_int = msg.data[0];
+                int  kp_M1_int = msg.data[1];
+                int  kp_M2_int = msg.data[2];
+                int  kp_H_int   = msg.data[3];
+                int  kd_int = msg.data[4];
+                int  range_L_int = msg.data[5];
+                int  range_M_int = msg.data[6];
+                int  range_H_int = msg.data[7];
+
     
+        controller->kp_L = uint_to_float(kp_L_int, kp_L_MIN, kp_L_MAX, 8);
+        controller->kp_M1 = uint_to_float(kp_M1_int, kp_M1_MIN, kp_M1_MAX, 8);
+        controller->kp_M2 = uint_to_float(kp_M2_int, kp_M2_MIN, kp_M2_MAX, 8);
+        controller->kp_H = uint_to_float(kp_H_int, kp_H_MIN, kp_H_MAX, 8);
+        controller->kd = uint_to_float(kd_int, KD_MIN, KD_MAX, 8);
+        controller->range_L = uint_to_float(range_L_int, range_L_MIN, range_L_MAX, 8);
+        controller->range_M = uint_to_float(range_M_int, range_M_MIN, range_M_MAX, 8);
+        controller->range_H = uint_to_float(range_H_int, range_H_MIN, range_H_MAX, 8);
+    //printf("Received   ");
+    //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");
+    }
+/*
 /// CAN Command Packet Structure ///
 /// 16 bit position command, between -4*pi and 4*pi
 /// 12 bit velocity command, between -30 and + 30 rad/s
@@ -67,5 +134,5 @@
     //printf("Received   ");
     //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");
-    }
+    }*/