|
|
Line 183: |
Line 183: |
| |} | | |} |
| <br> | | <br> |
− |
| |
− | ==Case-study: Softing CAN==
| |
− | In this chapter, sample code demonstrating the implementation of the CAN communication interface is provide. This is the foundation for Softing PCI CAN.
| |
− | ===Opening the CAN Communication Channel===
| |
− | <syntaxhighlight lang="cpp">
| |
− | char ch_name[256];
| |
− | sprintf_s(ch_name, 256, "CAN-ACx-PCI_%d", ch);
| |
− | INIL2_initialize_channel(&hCAN[ch-1], ch_name);
| |
− |
| |
− | L2CONFIG L2Config;
| |
− | L2Config.fBaudrate = 1000.0;
| |
− | L2Config.bEnableAck = 0;
| |
− | L2Config.bEnableErrorframe = 0;
| |
− | L2Config.s32AccCodeStd = 0;
| |
− | L2Config.s32AccMaskStd = 0;
| |
− | L2Config.s32AccCodeXtd = 0;
| |
− | L2Config.s32AccMaskXtd = 0;
| |
− | L2Config.s32OutputCtrl = GET_FROM_SCIM;
| |
− | L2Config.s32Prescaler = 1;
| |
− | L2Config.s32Sam = 0;
| |
− | L2Config.s32Sjw = 1;
| |
− | L2Config.s32Tseg1 = 4;
| |
− | L2Config.s32Tseg2 = 3;
| |
− | L2Config.hEvent = (void*)-1;
| |
− |
| |
− | CANL2_initialize_fifo_mode(hCAN[ch-1], &L2Config);
| |
− | </syntaxhighlight>
| |
− | ===CAN Initialization===
| |
− | <syntaxhighlight lang="cpp">
| |
− | long Txid;
| |
− | unsigned char data[8];
| |
− |
| |
− | Txid = ((unsigned long)ID_CMD_SET_PERIOD<<6) | ((unsigned long)ID_COMMON <<3) | ((unsigned long)ID_DEVICE_MAIN);
| |
− | data[0] = (unsigned char)period_msec;
| |
− | canWrite(hCAN, Txid, data, 1, STD);
| |
− |
| |
− | Sleep(10);
| |
− |
| |
− | Txid = ((unsigned long)ID_CMD_SET_MODE_TASK<<6) | ((unsigned long)ID_COMMON <<3) | ((unsigned long)ID_DEVICE_MAIN);
| |
− | canWrite(hCAN, Txid, data, 0, STD);
| |
− |
| |
− | Sleep(10);
| |
− |
| |
− | Txid = ((unsigned long)ID_CMD_QUERY_STATE_DATA<<6) | ((unsigned long)ID_COMMON <<3) | ((unsigned long)ID_DEVICE_MAIN);
| |
− | canWrite(hCAN, Txid, data, 0, STD);
| |
− | </syntaxhighlight>
| |
− | ===Starting Periodic CAN Communication===
| |
− | When you start periodic CAN communication, joint angles are automatically updated according to the torque control input.
| |
− |
| |
− | <syntaxhighlight lang="cpp">
| |
− | long Txid;
| |
− | unsigned char data[8];
| |
− |
| |
− | Txid = ((unsigned long)ID_CMD_QUERY_STATE_DATA<<6) | ((unsigned long)ID_COMMON <<3) | ((unsigned long)ID_DEVICE_MAIN);
| |
− | canWrite(hCAN[ch-1], Txid, data, 0, STD);
| |
− |
| |
− | Sleep(10);
| |
− |
| |
− | Txid = ((unsigned long)ID_CMD_SET_SYSTEM_ON<<6) | ((unsigned long)ID_COMMON <<3) | ((unsigned long)ID_DEVICE_MAIN);
| |
− | canWrite(hCAN[ch-1], Txid, data, 0, STD);
| |
− | </syntaxhighlight>
| |
− | ===Stopping Periodic CAN Communication===
| |
− | <syntaxhighlight lang="cpp">
| |
− | long Txid;
| |
− | unsigned char data[8];
| |
− |
| |
− | Txid = ((unsigned long)ID_CMD_SET_SYSTEM_OFF<<6) | ((unsigned long)ID_COMMON <<3) | ((unsigned long)ID_DEVICE_MAIN);
| |
− | canWrite(hCAN[ch-1], Txid, data, 0, STD);
| |
− | </syntaxhighlight>
| |
− | ===Transmitting Control Torques===
| |
− | Control inputs for the four joints in each finger should be packed in a single CAN frame. The sample code below demontrates how to encode four PWM inputs into an 8 byte data buffer and how to set the CAN frame ID properly.
| |
− |
| |
− | '''''Note:''' PWM = Desired_Torque (N-m) * '''800.0'''.<br>800.0 is an empirical constant that will convert torque to PWM.''<br>
| |
− | As seen below, ''torque2pwm = 800.0''
| |
− |
| |
− |
| |
− | '''''Note:''' The joint index order used in the following code is for '''Allegro Hand versions 2.0 and up'''. For Allegro hand 1.0 or earlier, see the code snippet after this one.''
| |
− |
| |
− | <syntaxhighlight lang="cpp">
| |
− | long Txid;
| |
− | unsigned char data[8];
| |
− | float torque2pwm = 800.0f
| |
− | short pwm[4] = {
| |
− | 0.1*torque2pwm,
| |
− | 0.1*torque2pwm,
| |
− | 0.1*torque2pwm,
| |
− | 0.1*torque2pwm
| |
− | };
| |
− |
| |
− | // This joint index order is used Allegro Hand versions 2.0 and up.
| |
− | if (findex >= 0 && findex < 4)
| |
− | {
| |
− | data[0] = (unsigned char)( (pwm[3] >> 8) & 0x00ff);
| |
− | data[1] = (unsigned char)(pwm[3] & 0x00ff);
| |
− |
| |
− | data[2] = (unsigned char)( (pwm[2] >> 8) & 0x00ff);
| |
− | data[3] = (unsigned char)(pwm[2] & 0x00ff);
| |
− |
| |
− | data[4] = (unsigned char)( (pwm[1] >> 8) & 0x00ff);
| |
− | data[5] = (unsigned char)(pwm[1] & 0x00ff);
| |
− |
| |
− | data[6] = (unsigned char)( (pwm[0] >> 8) & 0x00ff);
| |
− | data[7] = (unsigned char)(pwm[0] & 0x00ff);
| |
− |
| |
− | Txid = ((unsigned long)(ID_CMD_SET_TORQUE_1 + findex)<<6) | ((unsigned long)ID_COMMON <<3) | ((unsigned long)ID_DEVICE_MAIN);
| |
− | canWrite(hCAN, Txid, data, 8, STD);
| |
− | }
| |
− |
| |
− | </syntaxhighlight>
| |
− |
| |
− |
| |
− |
| |
− | '''''Note:''' The joint index order used in the following code is for '''Allegro Hand versions 1.0 and down'''. For Allegro hand 2.0 or later, see the code snippet before this one.''
| |
− |
| |
− | <syntaxhighlight lang="cpp">
| |
− | // This joint index order is used Allegro Hand versions 1.0 and down.
| |
− | if (findex >= 0 && findex < 4)
| |
− | {
| |
− | data[0] = (unsigned char)( (pwm[0] >> 8) & 0x00ff);
| |
− | data[1] = (unsigned char)(pwm[0] & 0x00ff);
| |
− |
| |
− | data[2] = (unsigned char)( (pwm[1] >> 8) & 0x00ff);
| |
− | data[3] = (unsigned char)(pwm[1] & 0x00ff);
| |
− |
| |
− | data[4] = (unsigned char)( (pwm[2] >> 8) & 0x00ff);
| |
− | data[5] = (unsigned char)(pwm[2] & 0x00ff);
| |
− |
| |
− | data[6] = (unsigned char)( (pwm[3] >> 8) & 0x00ff);
| |
− | data[7] = (unsigned char)(pwm[3] & 0x00ff);
| |
− |
| |
− | Txid = ((unsigned long)(ID_CMD_SET_TORQUE_1 + findex)<<6) | ((unsigned long)ID_COMMON <<3) | ((unsigned long)ID_DEVICE_MAIN);
| |
− | canWrite(hCAN, Txid, data, 8, STD);
| |
− | }
| |
− |
| |
− | </syntaxhighlight>
| |
− |
| |
− |
| |
− | ===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.
| |
− |
| |
− | <syntaxhighlight lang="cpp">
| |
− | 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, ¶m);
| |
− |
| |
− | 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)
| |
− | {
| |
− | 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];
| |
− | }
| |
− | }
| |
− |
| |
− | }
| |
− | </syntaxhighlight>
| |
| | | |
| ==Download== | | ==Download== |
| [[File:v4_AllegroHandUsersManual.pdf]] | | [[File:v4_AllegroHandUsersManual.pdf]] |
The CAN communication baud-rate is 1Mbps.
Messages can be sent to initialize or stop CAN communication.
The Allegro Hand control software attempts to communicate with the real or simulated hand at a regular control interval. Every 3 milliseconds, the joint torques are calculated and the joint angles are updated.
The standard CAN packet used for communication is 14 bytes including 8 bytes of data.
The 4 byte integer CAN message identifier (msg_id) is split into the command ID (26 bits), destination ID (3 bits) and source ID (3 bits).