Using 'Smart Control Servos' (Feetech) with Teensy...

Status
Not open for further replies.

JoMa

Member
Hello!

The smart control servos are really cool, because they have individual ID's so they can be used in a BUS network. There is a readPosition and writePosition function, that's why I want to use them.
The servos come with an arduino library (SCServo.h). The communitcation between arduino and the servos is over RX and TX. Between there is a TTLinker (Half Duplex Asynchronous Communication).

Here is my problem:
I need to change the communication from serial to serial1 or serial2. I guess I need to change the library. I tried without success. If anybody has an idea, I'd be very happy.
Thanks already for your time!

Here are the libraries:

SCServo.cpp
-----------------
/*
* SCServo.cpp
* Series Control Servo
* Created on: 2014.10.15
* Author: Tony tan
*/
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#define printf(args) (Serial.write(args))
#else
#include "WProgram.h"
#define printf(args) (Serial.print(args,BYTE))
#endif
#include "SCServo.h"

SCServo::SCServo ()
{
}

u8 SCServo::EnableTorque(u8 ID, u8 Enable, u8 ReturnLevel)
{
int messageLength = 4;

printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_TORQUE_ENABLE);
printf(Enable);
printf((~(ID + messageLength + INST_WRITE + Enable + P_TORQUE_ENABLE))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}

u8 SCServo:: WritePos(u8 ID, s16 position, s16 velocity, u8 ReturnLevel)
{
int messageLength = 7;
byte posL = position>>8;
byte posH = position&0xff;
byte velL = velocity>>8;
byte velH = velocity&0xff;

printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_GOAL_POSITION_L);
printf(posL);
printf(posH);
printf(velL);
printf(velH);
printf((~(ID + messageLength + INST_WRITE + P_GOAL_POSITION_L + posL + posH + velL + velH))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}

u8 SCServo:: RegWritePos(u8 ID, s16 position, s16 velocity, u8 ReturnLevel)
{
int messageLength = 7;
byte posL = position>>8;
byte posH = position&0xff;

byte velL = velocity>>8;
byte velH = velocity&0xff;

printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_REG_WRITE);
printf(P_GOAL_POSITION_L);
printf(posL);
printf(posH);
printf(velL);
printf(velH);
printf((~(ID + messageLength + INST_REG_WRITE + P_GOAL_POSITION_L + posL + posH + velL + velH))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}

void SCServo:: RegWriteAction()
{
int messageLength = 2;
byte ID = 0xFE;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_ACTION);
printf((~(ID + messageLength + INST_ACTION))&0xFF);
}

u8 SCServo:: ReadBuf(u8 len, u8 *buf)
{
u16 n = 0;
u8 size = 0;
u8 ComData;
while(n<TIMEOUT)
{
if(Serial.available())
{
if(buf)
buf[size] = Serial.read();
else
ComData = Serial.read();
size++;
if(size>=len)
break;
n = 0;
}
n++;
}
return size;
}

s16 SCServo:: ReadPos(u8 ID)
{
u8 buf[8];
u8 size;
u16 pos;
memset(buf,0,sizeof(buf));
printf(startByte);
printf(startByte);
printf(ID);
printf(4);
printf(INST_READ);
printf(P_PRESENT_POSITION_L);
printf(2);
printf((~(ID + 4 + INST_READ + P_PRESENT_POSITION_L + 2))&0xFF);
size = ReadBuf(8, buf);
if(size<8)
return -1;
pos = buf[5];
pos <<= 8;
pos |= buf[6];
return (s16)pos;
}

void SCServo:: SyncWritePos(u8 ID[], u8 IDN, s16 position, s16 velocity)
{
int messageLength = 5*IDN+4;
u8 Sum = 0;
byte posL = position>>8;
byte posH = position&0xff;

byte velL = velocity>>8;
byte velH = velocity&0xff;

printf(startByte);
printf(startByte);
printf(0xfe);
printf(messageLength);
printf(INST_SYNC_WRITE);
printf(P_GOAL_POSITION_L);
printf(4);

Sum = 0xfe + messageLength + INST_SYNC_WRITE + P_GOAL_POSITION_L + 4;
int i;
for(i=0; i<IDN; i++)
{
printf(ID);
printf(posL);
printf(posH);
printf(velL);
printf(velH);
Sum += ID + posL + posH + velL + velH;
}
printf((~Sum)&0xFF);
}

u8 SCServo:: WriteID(u8 oldID, u8 newID, u8 ReturnLevel)
{
int messageLength = 4;

printf(startByte);
printf(startByte);
printf(oldID);
printf(messageLength);
printf(INST_WRITE);
printf(P_ID);
printf(newID);
printf((~(oldID + messageLength + INST_WRITE + newID + P_ID))&0xFF);
if(oldID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}

u8 SCServo:: WriteLimitAngle(u8 ID, u16 MinAngel, u16 MaxAngle, u8 ReturnLevel)
{
int messageLength = 7;
byte MinAL = MinAngel>>8;
byte MinAH = MinAngel&0xff;
byte MaxAL = MaxAngle>>8;
byte MaxAH = MaxAngle&0xff;

printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_MIN_ANGLE_LIMIT_L);
printf(MinAL);
printf(MinAH);
printf(MaxAL);
printf(MaxAH);
printf((~(ID + messageLength + INST_WRITE + P_MIN_ANGLE_LIMIT_L + MinAL + MinAH + MaxAL + MaxAH))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}

u8 SCServo:: WriteLimitTroque(u8 ID, u16 MaxTroque, u8 ReturnLevel)
{
int messageLength = 5;
byte MaxTL = MaxTroque>>8;
byte MaxTH = MaxTroque&0xff;

printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_MAX_TORQUE_L);
printf(MaxTL);
printf(MaxTH);

printf((~(ID + messageLength + INST_WRITE + P_MAX_TORQUE_L + MaxTL + MaxTH))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}

u8 SCServo:: WritePunch(u8 ID, u16 Punch, u8 ReturnLevel)
{
int messageLength = 5;
byte PunchL = Punch>>8;
byte PunchH = Punch&0xff;

printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_PUNCH_L);
printf(PunchL);
printf(PunchH);

printf((~(ID + messageLength + INST_WRITE + P_PUNCH_L + PunchL + PunchH))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}

u8 SCServo:: WriteBaund(u8 ID, u8 Baund, u8 ReturnLevel)
{
int messageLength = 4;

printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_BAUD_RATE);
printf(Baund);

printf((~(ID + messageLength + INST_WRITE + P_BAUD_RATE + Baund))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}

u8 SCServo:: WriteComplianceMrgin(u8 ID, u8 CCW, u8 CW, u8 ReturnLevel)
{
int messageLength = 5;

printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_CW_COMPLIANCE_MARGIN);
printf(CW);
printf(CCW);

printf((~(ID + messageLength + INST_WRITE + P_CW_COMPLIANCE_MARGIN + CW + CCW))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}

u8 SCServo:: LockEprom(u8 ID, u8 Enable, u8 ReturnLevel)
{
int messageLength = 4;

printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_LOCK);
printf(Enable);

printf((~(ID + messageLength + INST_WRITE + P_LOCK + Enable))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}

u8 SCServo:: WritePID(u8 ID, u8 P, u8 I, u8 D, u8 ReturnLevel)
{
int messageLength = 6;

printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_COMPLIANCE_P);
printf(P);
printf(D);
printf(I);

printf((~(ID + messageLength + INST_WRITE + P_COMPLIANCE_P + P + D + I))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}

u8 SCServo:: WriteSpe(u8 ID, s16 velocity, u8 ReturnLevel)
{
int messageLength = 5;

u16 vel = velocity;
if(velocity<0)
{

vel = -velocity;
vel |= (1<<10);
}

byte velL = vel>>8;
byte velH = vel&0xff;

printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_GOAL_SPEED_L);
printf(velL);
printf(velH);
printf((~(ID + messageLength + INST_WRITE + P_GOAL_SPEED_L + velL + velH))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}


s16 SCServo:: ReadVoltage(u8 ID)
{
u8 buf[7];
u8 size;
s16 vol;
memset(buf,0,sizeof(buf));
printf(startByte);
printf(startByte);
printf(ID);
printf(4);
printf(INST_READ);
printf(P_PRESENT_VOLTAGE);
printf(1);
printf((~(ID + 4 + INST_READ + P_PRESENT_VOLTAGE + 1))&0xFF);
size = ReadBuf(7, buf);
if(size<7)
return -1;
vol = buf[5];
return vol;
}

s16 SCServo:: ReadTemper(u8 ID)
{
u8 buf[7];
u8 size;
s16 temper;
memset(buf,0,sizeof(buf));
printf(startByte);
printf(startByte);
printf(ID);
printf(4);
printf(INST_READ);
printf(P_PRESENT_TEMPERATURE);
printf(1);
printf((~(ID + 4 + INST_READ + P_PRESENT_TEMPERATURE + 1))&0xFF);
size = ReadBuf(7, buf);
if(size<7)
return -1;
temper = buf[5];
return temper;
}
------------

SCServo.h
------------
/*
* SCServo.h
* Series Control Servo
* Created on: 2014.10.15
* Author: Tony tan
*/
#ifndef _SCSERVO_h_
#define _SCSERVO_h_

#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif

#define s8 char
#define u8 unsigned char
#define u16 unsigned short
#define s16 short
#define u32 unsigned long
#define s32 long

class SCServo{
public:
SCServo();
u8 EnableTorque(u8 ID, u8 Enable, u8 ReturnLevel=2);
u8 WritePos(u8 ID, s16 position, s16 velocity, u8 ReturnLevel=2);
u8 RegWritePos(u8 ID, s16 position, s16 velocity, u8 ReturnLevel=2);
s16 ReadPos(u8 ID);
s16 ReadVoltage(u8 ID);
s16 ReadTemper(u8 ID);
void RegWriteAction();
void SyncWritePos(u8 ID[], u8 IDN, s16 position, s16 velocity);
u8 WriteID(u8 oldID, u8 newID, u8 ReturnLevel=2);
u8 WriteLimitAngle(u8 ID, u16 MinAngel, u16 MaxAngle, u8 ReturnLevel=2);
u8 WriteLimitTroque(u8 ID, u16 MaxTroque, u8 ReturnLevel=2);
u8 WritePunch(u8 ID, u16 Punch, u8 ReturnLevel=2);
u8 WriteBaund(u8 ID, u8 Baund, u8 ReturnLevel=2);
u8 WriteComplianceMrgin(u8 ID, u8 CCW, u8 CW, u8 ReturnLevel=2);
u8 WritePID(u8 ID, u8 P, u8 I, u8 D, u8 ReturnLevel=2);
u8 WriteSpe(u8 ID, s16 velocity, u8 ReturnLevel=2);
u8 LockEprom(u8 ID, u8 Enable, u8 ReturnLevel=2);
private:
u8 ReadBuf(u8 len, u8 *buf=NULL);
#define startByte 0xFF
#define TIMEOUT 2000//TIMEOUT 2000

#define B_1M 0
#define B_0_5M 1
#define B_250K 2
#define B_128K 3
#define B_115200 4
#define B_76800 5
#define B_57600 6
#define B_38400 7

//register Address
#define P_MODEL_NUMBER_L 0
#define P_MODEL_NUMBER_H 1
#define P_VERSION_L 3
#define P_VERSION_H 4
#define P_ID 5
#define P_BAUD_RATE 6
#define P_RETURN_DELAY_TIME 7
#define P_RETURN_LEVEL 8
#define P_MIN_ANGLE_LIMIT_L 9
#define P_MIN_ANGLE_LIMIT_H 10
#define P_MAX_ANGLE_LIMIT_L 11
#define P_MAX_ANGLE_LIMIT_H 12
#define P_LIMIT_TEMPERATURE 13
#define P_MAX_LIMIT_VOLTAGE 14
#define P_MIN_LIMIT_VOLTAGE 15
#define P_MAX_TORQUE_L 16
#define P_MAX_TORQUE_H 17
#define P_ALARM_LED 18
#define P_ALARM_SHUTDOWN 19
#define P_COMPLIANCE_P 21
#define P_COMPLIANCE_D 22
#define P_COMPLIANCE_I 23
#define P_PUNCH_L 24
#define P_PUNCH_H 25
#define P_CW_COMPLIANCE_MARGIN 26
#define P_CCW_COMPLIANCE_MARGIN 27

#define P_TORQUE_ENABLE (31)
#define P_LED (32)
#define P_GOAL_POSITION_L (33)
#define P_GOAL_POSITION_H (34)
#define P_GOAL_SPEED_L (35)
#define P_GOAL_SPEED_H (36)
#define P_LOCK (37)

#define P_PRESENT_POSITION_L (41)
#define P_PRESENT_POSITION_H (42)
#define P_PRESENT_SPEED_L (43)
#define P_PRESENT_SPEED_H (44)
#define P_PRESENT_LOAD_L (45)
#define P_PRESENT_LOAD_H (46)
#define P_PRESENT_VOLTAGE (47)
#define P_PRESENT_TEMPERATURE (48)
#define P_REGISTERED_INSTRUCTION (49)
#define P_ERROR (50)
#define P_MOVING (51)

//Instruction:
#define INST_PING 0x01
#define INST_READ 0x02
#define INST_WRITE 0x03
#define INST_REG_WRITE 0x04
#define INST_ACTION 0x05
#define INST_RESET 0x06
#define INST_SYNC_WRITE 0x83
};
#endif
-----------

This is one example:

writePosition.ino
-----------
#include <SCServo.h>

SCServo SERVO;

void setup()
{
Serial.begin(1000000);//init Serial baudrate
delay(500);
SERVO.EnableTorque(4, 1);
SERVO.EnableTorque(6, 1);
}

void loop()
{
SERVO.WritePos(4, 0x02FF, 100);// Servo ID:1, rotate to the position:0x2FF
SERVO.WritePos(6, 0x02FF, 100);// Servo ID:2, rotate to the position:0x2FF
delay(8000);
SERVO.WritePos(4,0x0000, 75);// Servo ID:1, rotate to the position:0x000
SERVO.WritePos(6, 0x0000, 75);// Servo ID:1, rotate to the position:0x000
delay(6000);
SERVO.WritePos(4, 0x02FF, 50);// Servo ID:1, rotate to the position:0x2FF
SERVO.WritePos(6, 0x02FF, 50);// Servo ID:2, rotate to the position:0x2FF
delay(4000);
SERVO.WritePos(4, 0x0000, 25);// Servo ID:1, rotate to the position:0x000
SERVO.WritePos(6, 0x0000, 25);// Servo ID:2, rotate to the position:0x000
delay(2000);
}
----------

With kind regards,
Johannes
 
Last edited:
Is there a github repository or website for this library? Some place the download the actual files, rather than extract from the message above?
 
From a very quick first look, it seems you should need to change this line:

Code:
#define printf(args) (Serial.write(args))

Have you tried just changing it to this?

#define printf(args) (Serial2.write(args))

You'll also need to change a three places inside ReadBuf

Code:
if(Serial.available())
{
if(buf)
buf[size] = Serial.read();
else
ComData = Serial.read();

Obviously, change each "Serial" to "Serial2".

Also, in setup(), change "Serial" to "Serial2".

Code:
Serial.begin(1000000);//init Serial baudrate
 
Thanks Paul for the answer!
Yes, I have tried to change all the "Serial" to "Serial2" But without success. Now and then the servo moves for a very short time (very rarely) I haven't found any pattern, yet. I'm thinking to get a logic level converter. I'm nut sure whether the TTLinker for the half duplex asynchronous communication can deal with the 3.3V UART of the teensy...
 
For those of you, who want to use the smart control servos as well: It works fine with teensy 3.1! My mistake was the 5V input on the TTLinker. With the arduino board it worked without connecting the 5V to anything. The TTLinker got its power from the Vin, that is used for driving the servos. Somehow this setup didn't work anymore with the teensy. Now I connected the 5V pin of the TTLinker to Vin of the teensy and did the adjustments Paul suggested and it works really nicely! So no logic level converter necessary! Thank you Paul for your feedback!
 
Last edited:
Site Down

Here you can download the library and find more information about the setup:
http://www.rcservozone.com/Wiki.jsp?page=Arduino library SCServo

Thanks for your help!

Hi

I just bought at starter kit for these servos. The kit says to got to rcsevozone.com to download setups software and Arduino library. The site has been down for sometime now, does anyone know of an alternate source for the windows setup software (I have sourced the Arduino linrary) and instructions?

Cheers
Stew
 
Stewart and other members in this thread...have you managed to find any information on the Feetech SCS15 servo. I have just purchased one as they had the specs that I wanted but have I spent good money on an unsupported unit. Does anyone have any schematic on how to connect the TTLinker/servo to an Arduino and the various libraries and examples needed. One YouTube video showed the servo being plugged into the TTLinker and 5v/Gnd connected to the other male socket instead of the 2-pin 5v/Gnd pins...very puzzling. Is anyone actually using one of these servo's in a working project who could help...I would be grateful
Thanks in advance
Brian
 
Brian
I did eventually get these servos to work, it was a long struggle. I eventually got in contact with someone at Feetech and they sent me what I needed. I have a technical document in Chinese which I have used Google Translate to translate to English, software for programming and using on windows along with libraries for Arduino. Would you like me to send you a copy?
Cheers
Stew
 
Yes please Stweart

Brian
I did eventually get these servos to work, it was a long struggle. I eventually got in contact with someone at Feetech and they sent me what I needed. I have a technical document in Chinese which I have used Google Translate to translate to English, software for programming and using on windows along with libraries for Arduino. Would you like me to send you a copy?
Cheers
Stew

Stewart...thanks for the offer...I would be most grateful for any information on this servo.
The courier service had lost my first order of this unit and I almost cancelled the purchase and start again with another brand but held out for the Feetech. Now you have it working...are you glad you chose that brand??
Look forward to any information

Thanks again
Airborne2
Brian
 
Yes, I am very happy with these servos.

I have attached 4 files;
SCS15 ManualEnglish.zip - A google translate version of Technical documentation.
SCServoArduinoLib.zip - Arduino libraries for the servo
FT-Servo Software and Driver V1.2(161207).zip - drivers and software to program the servos from Windows
SCS15 software V1.6.zip - an updated version of one of the programs from the above file. You need to install the one above first.

Let me know hoe you get on.

Cheers
Stew
 

Attachments

  • FT-Servo Software and Driver V1.2(161207).zip
    1.1 MB · Views: 321
  • SCServoArduinoLib.zip
    12.6 KB · Views: 290
  • SCS15 software V1.6.zip
    218.3 KB · Views: 288
  • SCS15 ManualEnglish.zip
    948.2 KB · Views: 539
how it works i download the library and i dont work in scs15 servo with ttlinker mini

how it works i download the library and i dont work in scs15 servo with ttlinker mini


Yes, I am very happy with these servos.

I have attached 4 files;
SCS15 ManualEnglish.zip - A google translate version of Technical documentation.
SCServoArduinoLib.zip - Arduino libraries for the servo
FT-Servo Software and Driver V1.2(161207).zip - drivers and software to program the servos from Windows
SCS15 software V1.6.zip - an updated version of one of the programs from the above file. You need to install the one above first.

Let me know hoe you get on.

Cheers
Stew
 
For sorry i dont have this adaptor i bought this servos with ttlinker_mini only and ineach motor the id is written on it
Sorry I don't have any of these or ever tried them...

Maybe might to help to know how you are connecting up the ttlinker_mini? Is it connecting somehow to a Teensy? My quick look through some of the stuff, looks like, probably connected to one of the Serial ports? Common ground? ...

Then I believe they all ship with ID #1, so you need to reprogram each of them with unique IDS...

Is there a forum specific to this manufacturer that you can ask questions and/or maybe has some tutorials on their servos?
 
The documentation for these servos is very poor. As you say they are all shipe with ID #1 and their baud rate is 1M. I have successfully got these servos to work with the SCPC-2 and a PC and using the TTL-mini with an Arduino, I have never used the Teensy.

Sorry I don't have any of these or ever tried them...

Maybe might to help to know how you are connecting up the ttlinker_mini? Is it connecting somehow to a Teensy? My quick look through some of the stuff, looks like, probably connected to one of the Serial ports? Common ground? ...

Then I believe they all ship with ID #1, so you need to reprogram each of them with unique IDS...

Is there a forum specific to this manufacturer that you can ask questions and/or maybe has some tutorials on their servos?
 
Status
Not open for further replies.
Back
Top