else
{index += 2;
}
}while(flags.nosync & index < recv_pkt_len);
While we’re parsing the received status message, we
might as well pick out some of the key data bytes and store
them for later use. You can choose the appropriate bytes
you need for your application. I decided to store the source
actuator ID and the status message ERROR byte.
If the nosync flag is cleared, the incoming status
message has passed the initial validity test. To determine
if the data we parsed from the status message is accurate,
we must check the validity of the incoming status
message’s checksum value:
//CHECK VALIDITY OF THE RECEIVED CHECKSUM
if(flags.nosync)
{returncode = 1;
//no sync characters
}
else if(flags.nosync == 0)
{calc_chk = 0;
for(index = recv_pkt_start + 2;
index < recv_pkt_len;++index)
{calc_chk += recv_buff[index];
}
recv_pkt_chksum = ~calc_chk;
if(recv_pkt_chksum !=
recv_buff[recv_pkt_len])
{returncode = 2; //bad status message
}
if(recv_pkt_error)
{if(returncode == 2)
{//bad checksum and error bits set
returncode = 4;
}
else
{//error byte has bits set
returncode = 3;
}
}
}
return returncode;
}
The get_packet function will return a zero if the status
36 SERVO 05.2009
message passes the sync character, ERROR byte, and
checksum validity tests. Otherwise, the return codes listed
in the function header will be returned to the caller.
Communicating with the
Dynamixel AX- 12+
Now that the core transmit and receive functions are in
place, we can start conversing with the robot actuators on
the link. For instance, here’s the actuator PING function:
//***********************************************
//* PING ROBOTIC ACTUATOR
//***********************************************
char ping_actuator (char act_id)
{char returncode;
send_packet(act_id,iPING,0x00);
mdelay1(1);
returncode = get_packet();
return returncode;
}
Let’s call it with this code snippet:
//***********************************************
//* MAIN MAIN MAIN MAIN MAIN
//***********************************************
void main()
{init();
cleaner();
rc = ping_actuator(0x01);
++scratch8; // debugger break point
}
The results of our PING can be seen in Screenshot 2.
The PING instruction indirectly operates against the
Dynamixel AX- 12+’s Control Table. Everything we do in the
PIC18F2620 firmware that deals directly with a robot
actuator accesses the robot actuator’s Control Table. So,
let’s write a function that retrieves all 50 of the Control
Table bytes from a Dynamixel AX- 12+:
//***********************************************
//* READ ENTIRE CONTROL TABLE
//***********************************************
char read_control_table (char act_id)
{char returncode;
parms[0] = 0x00; //start reading here
parms[1] = 0x32; //read this number of bytes
send_packet(act_id,iREAD_DATA,0x2);
mdelay1(1);
returncode = get_packet();
