Receiving Joint Angles - SimLab Allegro Hand User Manual

Receiving Joint Angles

Each finger consists of four joints. The joint angles for those four joints can be received via one CAN
packet. The sample code below demonstrates the method for decoding the data buffer and reading
the joint angles.
The sample code assumes that when fingers are in their zero positions, the joint angles from the CAN
packet are 32768. In practice, users should perform experiments and introduce offsets to obtain the
zero position.
Code 7: Receiving Joint Angles
char cmd;
char src;
char des;
int len;
unsigned char data[8];
int ret;
can_msg msg;
PARAM_STRUCT param;
ret = CANL2_read_ac(hCAN, &param);
switch (ret)
{
case CANL2_RA_DATAFRAME:
msg.msg_id = param.Ident;
msg.STD_EXT = STD;
msg.data_length = param.DataLength;
msg.data[0] = param.RCV_data[0];
msg.data[1] = param.RCV_data[1];
msg.data[2] = param.RCV_data[2];
msg.data[3] = param.RCV_data[3];
msg.data[4] = param.RCV_data[4];
msg.data[5] = param.RCV_data[5];
msg.data[6] = param.RCV_data[6];
msg.data[7] = param.RCV_data[7];
break;
}
cmd = (char)( (msg.msg_id >> 6) & 0x1f );
des = (char)( (msg.msg_id >> 3) & 0x07 );
src = (char)( msg.msg_id & 0x07 );
len = (int)( msg.data_length );
for(int nd=0; nd<len; nd++)
data[nd] = msg.data[nd];
switch (cmd)
{
case ID_CMD_QUERY_CONTROL_DATA:
{
if (id_src >= ID_DEVICE_SUB_01 && id_src <= ID_DEVICE_SUB_04)
{
}
}
}
Copyright 2008-2012 SimLab Co., Ltd. All rights reserved.
A L L E G R O H A N D U S E R S M A N U A L
int temp_pos[4]; // raw angle data
float ang[4]; // degree
float q[4]; // radian
temp_pos[0] = (int)(data[0] | (data[1] << 8));
temp_pos[1] = (int)(data[2] | (data[3] << 8));
temp_pos[2] = (int)(data[4] | (data[5] << 8));
temp_pos[3] = (int)(data[6] | (data[7] << 8));
ang[0] = ((float)(temp_pos[0]-32768)*(333.3f/65536.0f))*(1);
ang[1] = ((float)(temp_pos[1]-32768)*(333.3f/65536.0f))*(1);
ang[2] = ((float)(temp_pos[2]-32768)*(333.3f/65536.0f))*(1);
ang[3] = ((float)(temp_pos[3]-32768)*(333.3f/65536.0f))*(1);
q[0] = (3.141592f/180.0f) * ang[0];
q[1] = (3.141592f/180.0f) * ang[1];
q[2] = (3.141592f/180.0f) * ang[2];
q[3] = (3.141592f/180.0f) * ang[3];
30