Feetech Servos with a Teensy 4.1

mr_koz

Member
Greetings!

This is less asking for guidance and more sharing what I have figured out. Getting the Feetech servos running on my robot has been a bit challenging as there's not a lot of up to date information or source code available, added to my own blunders - this was a bit annoying!

I build robotic spiders, I have 4 models:
- Steve: arduino mega + 32 channel serial PWM servo controller + 24 hobby servos
- Yorick: Jetson nano + Pi + teensy 4.1 + rs485 MAX + 24 x rs485 dynamixels MX series servos
- Bobbie: Jetson nano + teensy 4.1 + ttl chip (forgot this one) + 24 x TTL dynamixel X series servos
- Stef: Pi + teensy 4.1 + Feetech TTL linker + 24 TTL micro Feetech brushless servos

The cost of the dynamixel servos is what led me to the Feetech servos, 2 of the MX64 servos cost more than 30 of the Feetech servos.

TLDR:

The TTL servos work a treat and are very fast. With a Teensy 4.1, the Feetech TTL Linker and 24 servos i got ~5200 read positions && some serial printlns per second.

The library source code is here: https://gitee.com/ftservo (you will need to sign up for an account)

The library source code needed adjustment for the servos I have - I switched the order of the data in SCS.readword. The code looked like this:

Code:
int SCS::readWord(u8 ID, u8 MemAddr)
{	
	u8 nDat[2];
	int Size;
	u16 wDat;
	Size = Read(ID, MemAddr, nDat, 2);
	if(Size!=2)
		return -1;
	wDat = SCS2Host(nDat[0], nDat[1]); // here!
	return wDat;
}

And i changed it to this:

Code:
int SCS::readWord(u8 ID, u8 MemAddr)
{	
	u8 nDat[2];
	int Size;
	u16 wDat;
	Size = Read(ID, MemAddr, nDat, 2);
	if(Size!=2)
		return -1;
	wDat = SCS2Host(nDat[1], nDat[0]); // here!
	return wDat;
}

I assume this same thing would have needed to be done on the writeWord but it seems to be fine so far.

A bit more detail

I found heaps of source code around that simply wasn't working, much of the code had Chinese symbols for comments and some of it had characters that VSCode/PIO couldn't render so Google Translate didn't help.

Hardware Layout

Very basic layout diagram - the key point to mention here is that i have powered the TTL Linker board from the 5v supply and not connected the battery to the TTL board, it works well this way and doesn't try and power 24 motors via the Linker board.

2023-04-25_14-37-57.png

Software

Code:
// Includes
#include <Arduino.h>
#include <stdint.h>
#include <SCServo.h>

// flow flashing lights
#define LED_PIN 13
bool ledState = LOW;
// debugging
char outputMessage[250];

// ttl baud rate
#define TTL_BAUD 1000000

// Power on the SSR for the motors
#define SSR_PIN 23

// Feetech servo controller
SCSCL FTServo;

// Servo IDs [%leg%joint]
int servoIds[24] = { 
  11,12,13, //  leg 1
  21,22,23, //  leg 2
  31,32,33, //  leg 3
  41,42,43, //  leg 4
  51,52,53, //  leg 5
  61,62,63, //  leg 6
  71,72,73, //  leg 7
  81,82,83, //  leg 8
};

// SYNC reads a specific servo position
int readPosition(int id, int waitTime = 0) {
  int position = FTServo.ReadPos(id);
  sprintf(outputMessage, "GET servo: %4d position: %4d", id, position);
  Serial.println(outputMessage);
  delay(waitTime);
  return position;
}

void gotoPosition(int id, int position) {
  sprintf(outputMessage, "SET servo: %4d position: %4d", id, position);
  Serial.println(outputMessage);

  FTServo.WritePos(id, position, 0, 2000);// Servo ID:1, rotate to the position:0x2FF
}

void motionTest(int servo_id, int delayTime=500) {
  u_int16_t targets[8] = {0, 1023, 2047, 3072, 4095, 3072, 2047, 1023};
  for (u_int8_t target = 0; target < 8; target++) {
    gotoPosition(servo_id, targets[target]); delay(delayTime);
    readPosition(servo_id); 
    delay(delayTime); 
  }
}

void perfTest(int loops = 3000) {
  // 3000x24 test loops = 13.6 seconds (with serial.print statements turned off) - this has a SYNC read position
  int start = millis();

  for (int i = 0; i < loops; i++) {
    for (int ii = 0; ii < 24; ii++) {
      readPosition(servoIds[ii]);
    }
  }

  sprintf(outputMessage, "test time took: %lu", millis() - start);
  Serial.println(outputMessage);
}

void ping(int id) {
  int response = FTServo.Ping(id);

  if(response == -1) sprintf(outputMessage, "Ping Failed: %d", id);
  else sprintf(outputMessage, "Ping Succeeded: %d", id);
  
  Serial.println(outputMessage);
  delay(200);
}

void setup() {
  // SSR high pin
  pinMode(SSR_PIN, OUTPUT);
  digitalWrite(SSR_PIN,true);
  // Loop flash
  pinMode(LED_PIN, OUTPUT);
  // Connect serial for debug
  Serial.begin(192000);

  // Connect serial for ttl
    Serial.println("Serial connected, connecting TTL");
  Serial2.begin(TTL_BAUD);
  FTServo.pSerial = &Serial2;

    delay(500);
}

void loop() {
  Serial.println("loop");
  digitalWrite(LED_PIN, ledState);
  ledState = !ledState;

  // perfTest();
  
  delay(500);
}

It's not walking yet but it's looking good!

20230425_144615_01_small.jpg

Happy to answer questions :)
 
Back
Top