AK80-64 MIT mode cannot control

Actuators
1

My situation:

  1. Hardware Version 2.1 and Software Version 1.1
  2. I have set CAN ID is 0x02 by R-Link and Testing MIT mode via “Cubemars Upper computer” ( My power supply is 30V 10A.) It works fine.

messageImage_1664123594750
messageImage_16641235947501333×1000 471 KB

I have AK80-64 and STM32F103 (bluepill) which have CAN controller on board and CAN Transceiver module.

I can read the position, speed, and torque of the motor by CAN bus, like the figure below.

unnamed

Issue:
I cannot control the MIT mode position or speed control. Even though I have followed the manual 1.0.9 version.

My Code:



float p_in = 0.0f;
float v_in = 0.0f;
float kp_in = 0.0f;
float kd_in = 0.0f;
float t_in = 0.0f;

/// limit data to be within bounds ///
float P_MIN =	-12.5f;
float P_MAX =	12.5f;
float V_MIN =	-8.0f;
float V_MAX =	8.0f;
float T_MIN =	-144.0f;
float T_MAX =	144.0f;
float Kp_MIN = 0.0f;
float Kp_MAX = 500.0f;
float Kd_MIN = 0.0f;
float Kd_MAX = 5.0f;

unsigned int p_int;
unsigned int v_int;
unsigned int kp_int;
unsigned int kd_int;
unsigned int t_int;

float position;
float speed;
float torque;
//MIT mode
void pack_cmd(float p_des, float v_des, float kp, float kd, float t_ff)
{
	p_in = fminf(fmaxf(P_MIN, p_des), P_MAX);
	v_in = fminf(fmaxf(V_MIN, v_des), V_MAX);
	kp_in = fminf(fmaxf(Kp_MIN, kp), Kp_MAX);
	kd_in = fminf(fmaxf(Kd_MIN, kd), Kd_MAX);
	t_in = fminf(fmaxf(T_MIN, t_ff), T_MAX);


	/// convert floats to unsigned ints ///
	 p_int = float_to_uint(p_in, P_MIN, P_MAX, 16);
	 v_int = float_to_uint(v_in, V_MIN, V_MAX, 12);
	 kp_int = float_to_uint(kp_in, Kp_MIN, Kp_MAX, 12);
	 kd_int = float_to_uint(kd_in, Kd_MIN, Kd_MAX, 12);
	 t_int = float_to_uint(t_in, T_MIN, T_MAX, 12);


	/// pack ints into the can buffer /// I set motor’s ID = 2
	TxHeader.StdId = 0x02;
	TxHeader.IDE = CAN_ID_STD;
	TxHeader.RTR = CAN_RTR_DATA;
	TxHeader.DLC = 8;

	TxData[0] = p_int >> 8;		
	TxData[1] = p_int & 0xFF;		
	TxData[2] = v_int >> 4;		
	TxData[3] = ((v_int & 0xF) << 4) | (kp_int >> 8); 	
	TxData[4] = kp_int & 0xFF;		
	TxData[5] = kd_int >> 4;		
	TxData[6] = ((kd_int & 0xF) << 4) | (kp_int >> 8);	
	TxData[7] = t_int & 0xff;	

	HAL_CAN_AddTxMessage(&hcan, &TxHeader, TxData, &TxMailbox);
	HAL_Delay(2);
}
void Enable_motor()
{
  	//Enable Motor
	TxHeader.StdId = 0x02;  		//Motor's ID
	TxHeader.IDE = CAN_ID_STD;
	TxHeader.RTR = CAN_RTR_DATA;
	TxHeader.DLC = 8;

	TxData[0] = 0XFF;
	TxData[1] = 0xFF;
	TxData[2] = 0xFF;
	TxData[3] = 0xFF;
	TxData[4] = 0xFF;
 	TxData[5] = 0xFF;
 	TxData[6] = 0xFF;
 	TxData[7] = 0xFC;

 	HAL_CAN_AddTxMessage(&hcan, &TxHeader, TxData, &TxMailbox);
 	HAL_Delay(2);
}
void Disable_motor()
{
  	//Enable Motor
	TxHeader.StdId = 0x02;  		//Motor's ID
	TxHeader.IDE = CAN_ID_STD;
	TxHeader.RTR = CAN_RTR_DATA;
	TxHeader.DLC = 8;

	TxData[0] = 0XFF;
	TxData[1] = 0xFF;
	TxData[2] = 0xFF;
	TxData[3] = 0xFF;
	TxData[4] = 0xFF;
 	TxData[5] = 0xFF;
 	TxData[6] = 0xFF;
 	TxData[7] = 0xFD;

 	HAL_CAN_AddTxMessage(&hcan, &TxHeader, TxData, &TxMailbox);
 	HAL_Delay(2);
}

void SetZeroPos()
{
  	//Enable Motor
	TxHeader.StdId = 0x02;  		//Motor's ID
	TxHeader.IDE = CAN_ID_STD;
	TxHeader.RTR = CAN_RTR_DATA;
	TxHeader.DLC = 8;

	TxData[0] = 0XFF;
	TxData[1] = 0xFF;
	TxData[2] = 0xFF;
	TxData[3] = 0xFF;
	TxData[4] = 0xFF;
 	TxData[5] = 0xFF;
 	TxData[6] = 0xFF;
 	TxData[7] = 0xFE;

 	HAL_CAN_AddTxMessage(&hcan, &TxHeader, TxData, &TxMailbox);
 	HAL_Delay(2);
}
int float_to_uint(float x, float x_min, float x_max, unsigned int bits)
{
	float span = x_max - x_min;
	if(x < x_min)
		x = x_min;
	else if(x > x_max)
		x = x_max;
	return (int) ((x- x_min)*((float)((1<<bits)/span)));
}
int main(void)
{
  SetZeroPos();
  Enable_motor();
  While(1)
  {  
      pack_cmd(1.57, 0.0, 1.0, 0.0, 0.0); //Set position 90 Degree Kp = 1.0
  }
}