Diff: CAN/CAN_com.cpp

Revision:

56:542e99ed5565

Parent:

55:b97b90d06ffa

Child:

59:d53a7ccaae9a

diff -r b97b90d06ffa -r 542e99ed5565 CAN/CAN_com.cpp
--- a/CAN/CAN_com.cpp	Tue May 25 11:43:53 2021 +0000
+++ b/CAN/CAN_com.cpp	Fri Jun 18 14:22:20 2021 +0000
@@ -40,6 +40,9 @@
     msg->data[5] = t_int&0xFF;
     // --------------------------
     */
+    
+    /*
+    // ----- fixed pid controller parameters(set at menu) -----
     int p_int = float_to_uint(p,P_REPLY_MIN,P_REPLY_MAX,16);
     int t_int = float_to_uint(t, T_MIN, T_MAX, 12);
     
@@ -50,8 +53,22 @@
     msg->data[4] = t_int&0xFF;
     //printf("position: %.3f ", p);
     //printf("\n\r");
+    */
     
-    }
+    // ----- changable kp with fixed kd -----
+    int p_int = float_to_uint(p, P_REPLY_MIN, P_REPLY_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);
+    msg->data[0] = CAN_ID;
+    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 ///
 /// 16 bit position command, between -4*pi and 4*pi
@@ -89,11 +106,25 @@
         controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12);
         // -------------------------------
         */
+    /*
+    // ----- fixed pid controller parameters(set at menu) -----
+    // only send position command and torque command
     int p_int = (msg.data[0]<<8)|msg.data[1];
     int t_int = (msg.data[2]<<8)|msg.data[3];
     
     controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16);
     controller->t_ff = uint_to_float(t_int, T_MIN, T_MAX, 12);
+    //
+    */
+    
+    // ----- changable kp with fixed kd -----
+    int p_int = (msg.data[0]<<8)|msg.data[1];
+    int kp_int = (msg.data[2]<<4)|(msg.data[3]>>4);
+    int t_int = ((msg.data[3]&0xF)<<8)|msg.data[4];
+    
+    controller->p_des = uint_to_float(p_int, P_MIN, P_MAX, 16);
+    controller->kp = uint_to_float(kp_int, KP_MIN, KP_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);