Revision 12:2aa99d54af80, committed 2020-11-13

Comitter:

indolcid

Date:

Fri Nov 13 02:38:14 2020 +0000

Parent:

11:58a70b97c750

Commit message:

Modified velocity control command; - Desired Velocity + Torque F/F

Changed in this revision

--- a/main.cpp	Mon Oct 19 23:29:46 2020 +0000
+++ b/main.cpp	Fri Nov 13 02:38:14 2020 +0000
@@ -28,10 +28,6 @@
  #define KD_MAX 5.0f
  #define T_MIN -18.0f
  #define T_MAX 18.0f
- #define P_GAIN_MIN 0.0f
- #define P_GAIN_MAX 2048.0f
- #define I_GAIN_MIN 0.0f
- #define I_GAIN_MAX 2048.0f
  /// Joint Soft Stops ///
  #define A_LIM_P 1.5f
  #define A_LIM_N -1.5f
@@ -138,15 +134,15 @@
 
 void pack_cmd2(CANMessage * msg, joint_control joint){
      
-     /// limit data to be within bounds ///
-     float v_des = fminf(fmaxf(V_MIN, joint.v_des), V_MAX);
-     /// pack ints into the can buffer ///
-     uint16_t kp = (uint16_t)(fminf(fmaxf(P_GAIN_MIN, joint.kp), P_GAIN_MAX) * 32);
-     uint16_t ki = (uint16_t)(fminf(fmaxf(I_GAIN_MIN, joint.kd), I_GAIN_MAX) * 32);
+     int16_t v_des = (int16_t)fminf(fmaxf(-32738.0f, joint.v_des * 128.0f), 32767.0f);
+     int16_t t_ff = (int16_t)fminf(fmaxf(-32738.0f, joint.t_ff * 128.0f), 32767.0f);
+     uint8_t kp = (uint8_t)fminf(fmaxf(0.0f, joint.kp * 16.0f), 255.0f);
+     uint8_t ki = (uint8_t)fminf(fmaxf(0.0f, joint.kd * 16.0f), 255.0f);
      
-     memcpy(msg->data, &v_des, 4);
-     memcpy(msg->data + 4, &kp, 2);
-     memcpy(msg->data + 6, &ki, 2);
+     memcpy(msg->data, &v_des, 2);
+     memcpy(msg->data + 2, &t_ff, 2);
+     memcpy(msg->data + 4, &kp, 1);
+     memcpy(msg->data + 5, &ki, 1);
      }
      
 /// CAN Reply Packet Structure ///
@@ -367,8 +363,19 @@
                 printf("+");
                 Zero(&h1_can);
                 Zero(&h2_can);
+
+                l1_control.k.p_des = 0.0f;
                 l1_control.k.v_des += PI * 2.0f * 0.1f;
+                l1_control.k.kp = 4.0f;
+                l1_control.k.kd = 0.5f;
+                l1_control.k.t_ff = 0.0f;
+
+                l2_control.k.p_des = PI * 2.0f * 0.1f;
                 l2_control.k.v_des += PI * 2.0f * 0.1f;
+                l2_control.k.kp = PI * 4.0f;
+                l2_control.k.kd = PI * 0.5f;
+                l2_control.k.t_ff = PI * 2.0f * 0.1f;
+
                 pack_cmd2(&k1_can, l1_control.k);
                 pack_cmd2(&k2_can, l2_control.k);
                 break;
@@ -376,8 +383,19 @@
                 printf("-");
                 Zero(&h1_can);
                 Zero(&h2_can);
+
+                l1_control.k.p_des = 0.0f;
                 l1_control.k.v_des -= PI * 2.0f * 0.1f;
+                l1_control.k.kp = 4.0f;
+                l1_control.k.kd = 0.5f;
+                l1_control.k.t_ff = 0.0f;
+
+                l2_control.k.p_des = 0.0f;
                 l2_control.k.v_des -= PI * 2.0f * 0.1f;
+                l2_control.k.kp = 4.0f;
+                l2_control.k.kd = 0.5f;
+                l2_control.k.t_ff = 0.0f;
+
                 pack_cmd2(&k1_can, l1_control.k);
                 pack_cmd2(&k2_can, l2_control.k);
                 break;