I bought Feetech SCS09 smart servos (also known as SCS009 or SCS0009).
Please note that these servos are not regular PWM servos, they are devices that communicate with board using Serial connection via device named TTLinker that converts UART to half duplex protocol. So the question is about the problem with serial communication only.
The seller provided me Arduino library to work with the servos.
The simplest testing sketch looks as this:
#include <SCServo.h>
SCServo SERVO;
void setup() {
Serial2.begin(1000000);
SERVO.pSerial = &Serial2;
delay(500);
SERVO.EnableTorque(1, 1);
SERVO.EnableTorque(2, 1);
}
void loop() {
SERVO.WritePos(1, 1023, 4000); // Servo ID:1, rotate to the position:0x2FF
SERVO.WritePos(2, 1023, 1000); // Servo ID:2, rotate to the position:0x2FF
delay(4000);
SERVO.WritePos(1, 20, 3000); // Servo ID:1, rotate to the position:0x000
SERVO.WritePos(2, 20, 1000); // Servo ID:1, rotate to the position:0x000
delay(3000);
}
The code works perfectly on Arduino Mega and allows to independently control servos.
But with Due I have problems: not all commands can be performed. So if compile the code for Due only servo ID1 works, but ID2 do not work at all.
When I add delays between commands the servos started to work
SERVO.WritePos(1, 1023, 4000); // Servo ID:1, rotate to the position:0x2FF
delay(10);
SERVO.WritePos(2, 1023, 1000); // Servo ID:2, rotate to the position:0x2FF
delay(4000);
SERVO.WritePos(1, 20, 3000); // Servo ID:1, rotate to the position:0x000
delay(10);
SERVO.WritePos(2, 20, 1000); // Servo ID:1, rotate to the position:0x000
delay(3000);
When I set delays between 1 and 9 the both servos can work but sometimes skip commands. Servos can skip 1-4 actions and then start to work again. The less delay I set the more random skips I get.
So I think here is some specialities regarding Serial connection on Arduino Due. What It can be? Why delays help to make it working?
By the way I read that Arduino Due can have problems with 1000000 baud rate and I tried different, even significntly less baud rates (for example 38400) with Arduino Due: the result is the same.
I need to explain how WritePos function works in the library
int SCSProtocol::WritePos(u8 ID, u16 Position, u16 Time, u16 Speed)
{
return writePos(ID, Position, Time, Speed, INST_WRITE);
}
int SCSProtocol::writePos(u8 ID, u16 Position, u16 Time, u16 Speed, u8 Fun)
{
flushSCS();
u8 buf[6];
Host2SCS(buf+0, buf+1, Position);
Host2SCS(buf+2, buf+3, Time);
Host2SCS(buf+4, buf+5, Speed);
writeBuf(ID, P_GOAL_POSITION_L, buf, 6, Fun);
return Ack(ID);
}
void SCSProtocol::Host2SCS(u8 *DataL, u8* DataH, int Data)
{
if(End){
*DataL = (Data>>8);
*DataH = (Data&0xff);
}else{
*DataH = (Data>>8);
*DataL = (Data&0xff);
}
}
int SCSProtocol::Ack(u8 ID)
{
if(ID != 0xfe && Level){
u8 buf[6];
u8 Size = readSCS(buf, 6);
if(Size!=6){
return 0;
}
}
return 1;
}
void SCSProtocol::writeBuf(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen, u8 Fun)
{
u8 msgLen = 2;
u8 bBuf[6];
u8 CheckSum = 0;
bBuf[0] = 0xff;
bBuf[1] = 0xff;
bBuf[2] = ID;
bBuf[4] = Fun;
if(nDat){
msgLen += nLen + 1;
bBuf[3] = msgLen;
bBuf[5] = MemAddr;
writeSCS(bBuf, 6);
}else{
bBuf[3] = msgLen;
writeSCS(bBuf, 5);
}
CheckSum = ID + msgLen + Fun + MemAddr;
u8 i = 0;
if(nDat){
for(i=0; i<nLen; i++){
CheckSum += nDat[i];
}
}
writeSCS(nDat, nLen);
writeSCS(~CheckSum);
}
int SCServo::readSCS(unsigned char *nDat, int nLen)
{
int Size = 0;
int ComData;
unsigned long t_begin = millis();
unsigned long t_user;
while(1){
ComData = pSerial->read();
if(ComData!=-1){
if(nDat){
nDat[Size] = ComData;
}
Size++;
t_begin = millis();
}
if(Size>=nLen){
break;
}
t_user = millis() - t_begin;
if(t_user>IOTimeOut){
break;
}
}
return Size;
}
int SCServo::writeSCS(unsigned char *nDat, int nLen)
{
if(nDat==NULL){
return 0;
}
return pSerial->write(nDat, nLen);
}
So the problem is somewhere here: Arduino Mega works good, but it seems like Arduino Due have no time for on of the operations.
You can see more info here:
