that is what the U2STAbits. TRMT bit
tells us. We don’t want to flip the
circuit to receive data while we are
still transmitting, so we look for the
transmission to be complete.
When complete, we flip the
~Tx/Rx line high to receive. I have
found that the act of flipping from
Tx to Rx causes a false UART start bit
to be seen which always gave me an
extra 0xFF at the start of the status
packet. My (less than perfect)
solution at the moment is to
immediately flush the UART receive
buffer by reading a byte.
The next section looks for the
start of a status response packet,
which is two 0xFF bytes. If it doesn’t
get the first of these bytes within
500 µs, we will time out and return
an error token, 0xFF. The function
then starts another loop that gets the
rest of the status packet. It too will
time out if this phase doesn’t
Listing 3: SendCmd function.
uint8_t SendCmd(uint8_t *cmd, uint8_t cLen, uint8_t *stat, uint8_t
*sLen)
/*
* Send Dynamixel command, return status in the cmd array
* INPUT: cmd[] to send , cLen is length of command
* OUPUT: stat[] status returned , sLen is length of status
*
* RETURNS: non-zero if an error
*/
{
uint8_t n = 0;
uint32_t t;digitalWrite(nTxU2, LOW);
Serial1.write(cmd, cLen);
// writing out cmd
while(U2STAbits.TRMT == 0);
digitalWrite(nTxU2, HIGH);
Serial1.read();
t = micros() + 500;
while (micros() < t)
{
if (Serial1.available())
{
stat[0] = Serial1.read();
if (stat[0] = 0xFF)
{
n++;
break;
}
}
}
// Wait for all bytes to go out
// time to read back status
// switch creates false start bit
// wait for reception to start
if (n == 0)
{
return(0xFF);
*sLen = 0;
}
// never got start byte
// error flag
t = micros() + 500;
while(micros() < t)
{
// Get the rest of the packet
while(Serial1.available() > 0)
{
stat[n++] = Serial1.read();
}complete within 500 µs.
In reality, the majority of the
status packets received will be under
10 bytes — which at 10 µs per byte is
under 100 µs. This is a comfortably
conservative timeout. Best of all, it
seems to work great with the 1-3
servo busses that I’ve tested it with.
The default AX12a response
delay is 160 µs, but this is a
configurable control table parameter,
Until next month, keep on
building robots! SV
}
*sLen = n;
// number of bytes in the status
return(stat[4]);
}
// return the error byte
14 SERVO 06.2013
