AK Series CAN protocol Instruction 코드 정리(2)

배경

T-motor 사에서 제공하는 AK 모터를 CAN 통신을 사용해 제어하는 매뉴얼을 코드위주로 정리했습니다.

초기 모터-컴퓨터 연결 계획도

---

Send Routine Code

void pack_cmd(CANMessage *msg, float p_des, float v_des, float kp, float kd, float t_ff){
// limit data to be within bounds 

float P_MIN=-95.5;
float P_MAX = 9505;
float V_MIN = -30;
float V_MAX = 30;
float T_MIN = -18;
float T_MAX = 18;
float Kp_MIN = 0;
float Kp_MAX = 500;
float Kd_MIN = 0;
float Kd_MAX = 5;
float Test_Post = 0.0;

p_des = fminf(fmaxf(P_MIN, p_des), P_MAX);
v_des = fminf(fmaxf(V_MIN, v_des), V_MAX);
kp = fminf(fmaxf(Kp_MIN, kp), Kp_MAX);
kd = fminf(fmaxf(Kd_MIN, kd), Kd_MAX);
t_ff = fminf(fmaxf(T_MIN, t_ff), T_MAX);

//convert floats to unsigned ints(function explained below)
int p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);
int v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
int kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
int kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
int t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);

// pack ints into the buffer
msg->data[0] = p_int >> 8; //position 8-H
msg->data[1] = p_int&0xFF; //position 8-L
msg->data[2] = v_int>>4; //speed 8-H
msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8); //speed 4-L KP 8-H
msg->data[4] = kp_int&0xFF; //KP 8-L
msg->data[5] = kd_int>>4; //KD 8-H
msg->data[6] = ((kd_int&0xF)<<4|(t_int>>8); //KD 4-L torque 4-H
msg->data[7] = t_int&0xFF; //torque 8-L
}

 

// limit data to be within bounds 

desired 값의 범위를 MIN, MAX 값을 통해 제한하며 각 AK 모터가 감당할 수 있는 부하 내에서 동작하도록 설정하는 과정입니다. 각각 fminf, fmaxf 함수는 두 개의 부동소수점을 포함한 인자를 받아 최솟값 혹은 최댓값을 반환합니다. 

 

---

 

// convert floats to unsigned ints

각 실수형으로 받은 인자들을 unsinged int 형식으로 바꿔주는 과정을 진행합니다. 코드는 아래와 같습니다. 

int float_to_unit(float x, float x_min, float x_max, unsigned int bits){

float span = x_max - x_min; //(1)
if(x < x_min) x = x_min; //(2)
else if(x > x_max) x = x_max; //(3)

return (int) ((x-x_min)*((float)((1<<bits)-1)/span));//(4)
}

1. 최댓값 - 최솟값을 구해 전체 범위 값(span)을 구합니다
2. 설정한 최솟값보다는 크도록 조건을 줍니다
3. 설정한 최댓값 보다는 작도록 조건을 줍니다
4. 비트 값을 반환합니다
   1. (x - x_min) : desired 값이 최솟값으로부터 얼마나 떨어져 있는지 구합니다
   2. /span : 위의 값을 전체 범위로 나누어 전체 범위 중 desired 값의 위치를 알려줍니다
   3. (1<<bits)-1 : 변환할 bit 값 형태를 알려줍니다(ex bits=8이라면 왼쪽 계산식은 255를 반환) 

 

---

// pack ints into buffer

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

1. p_int(16bits) 값을 왼쪽으로 8bit shift 시켜 상위 8bit 값만 저장합니다
2. 0xFF(= 0000 0000 1111 1111)와 p_int 값을 and 연산을 통해 하위 8bit를 추출합니다
3. v_int(12bits) 값을 왼쪽으로 4bit shift 시켜 상위 8bit 값만 저장합니다
4. v_int 하위 4bit 값과 kp_int 상위 4bit 값을 포함한 data[3] 값을 저장합니다(아래 설명)
   1. 0xF(=0000 0000 1111)와 v_int를 and 연산시켜 하위 4bit를 남깁니다
   2. 그 후 4bit left shift를 통해 data [3]의 하위 4비트 자리를 만듭니다.
   3. kp_int(12bit) 를 8bit right shift를 통해 상위 4bit를 남깁니다. 
   4. 2,3번 과정을 통해 남은 값들을 or 연산을 통해 합친 후 data [3]에 저장합니다.
5. 아래 과정은 위와 거의 동일하여 생략하겠습니다(아래 Data Frame 참조)

사용자->모터

 

---