Uncategorized

FeeTech SCS Servo Protocol Library

@AdrianMaire Ha, I have not thought about it too carefully. I simple mindedly thought that higher clock speed means less time each instruction. So 2GHz CPU should run instructions two times faster than 1GHz CPU. I have not thought about the OS scheduling. I need to think again.  1 day later… Adrian Maire13.4kThu 5:46Here is the library at a “workable” state. It implements all(*) commands and has a simple command-line tool to move and check Vin and position.
(*) Unfortunately, I am unable to make the MOVE_TIME_WAIT_WRITE, MOVE_TIME_WAIT_READ, MOVE_START and MOVE_STOP commands to work, it seems like they are not implemented? or maybe documentation is deprecated.. Not sure.github.com/Escain/HiwonderRPI/…Let me know if you manage to make those commands to workI also received the 555 chips, so I will try to replace the board with my own circuit. Soon more!  11 hours later… tlfong014137Thu 16:43Congratulations and many thanks for the library. So I can use command line tools to do the following: “a simple command-line tool to move and check Vin and position”?I am going very slow. I only know how to use python to read Vin, with intermittent read errors. So I am writing a check sum function and use it to resend readVin command and calculate the error read rate.I skimmed your source code and found the section of code seemingly to read Vin:

	else if (command == "read_voltage")
	{
		if (!checkArguments(num, 1, "read_voltage")) return 1;

		auto idOpt = getServoId(argsStr[2],1);
		if (!idOpt) return 1;

		HiwonderRpi::HiwonderBusServo servo(*idOpt);
		std::cout << "    " << static_cast<float>(servo.vinRead())/1000.0f << "V" << std::endl;
	}

But I could not any checksum code to steal or borrow. Have you written any check sum function?I skimmed to found code segment seemingly to do delay 3000 or 200 something. I guess those are for delay of servo operation, say to move to a particular position. But is the 200 something 200us, the command and reply gap?Also are you using your original HC126 cct with enable/disable TxD/RxD, or your new version with Rpi 3V3/5V0 level shift up? My cct does not sue any GPIO to pull up HC03 and CD4050. So it is unlikely that I can test your code with my Rpi. Anyway, perhaps you can confirm your modified HC126 cct first, before I try the code. There are other difficulties I might need to tackle before trying your code. More details later. Keep up with your nice work! Cheers. Adrian Maire13.4kThu 17:26“But I could not any checksum code to steal or borrow. Have you written any check sum function?”
This file just access the library (you will find nothing interesting inside). Look at src/HiwonderBusServo.hpp, there is all the “real” code.
You will find a “checksum” function around line 272“But is the 200 something 200us, the command and reply gap?”
The delays are in ms, and is to wait the servo finish to move before to release UART (this is for the command line only, not for the library)“Also are you using your original HC126 cct with enable/disable TxD/RxD”
Currently, the library was tested just with the Hiwonder Board. I made the 3.3V to 5V conversion with two HC126, but I do not enable/disable Tx / RX, as the board does the half-duplex to full-duplex conversion. Basically it is just the circuit that I hand-drawed few messages ago“I am going very slow”
Please, don’t feel any pressure: few week ago you advanced a lot while I was doing nearly nothing. As you say: “nothing urgent in our exploratory/feasibility project. As we are newbies in bus servo, we don’t need to setup any goal, not to mention any spec or schedule. I used to think we are using an Agile style, with frequent short meetings/chats and change our direction and goal time after time.”  4 hours later… tlfong014137Thu 21:27@AdrianMaire Ah, so you mean you are not writing your own library but just assessing the open source library, no wonder I could not find any checksum functions in your GitHub files. Actually I got stuck in a small part of the checksum algorithm (about the bitwise negation, which I wrong thought was about two’s complement). Now I think I found what my function went wrong and the bug should be fixed easily.@AdrianMaire I see. So you confirm the delay is for the servo to do its job. I have not gone that far yet, and will check out later.@AdrianMaire I see, I missed the critical part that you are using the HiWonder board to do testing. So you will be using the 555 timer to do the enable/disable Tx/Rx thing. Good luck. tlfong014137Thu 21:44@AdrianMaire Ah yes, no problem and no pressure at all. The reason that I told you that I am going very slowly, is to let you know don’t expect me catching up fast. I remember last time I told you that I found you reach a milestone and I must catch up, that was a joke, because I already caught up and so told you the joke.I see our project a fun game. My ex favorite sport was squash. My regular partners almost always beat me up badly, but I never gave up and still enjoyed the game, because I enjoyed discovering how to improve my techniques, and most importantly I, in other times would find some weaker players who wrongly thought could beat me up, but later found I surprisingly beat them up badly.It is just like program debugging or playing chess, not with a dead line or schedule. Ah, almost bed time! Happy learning together. See you later. Cheers.Last time I mentioned the following: “I googled further and found the following very interesting: alibaba.com/product-detail/….” It is about another serial bus servo by FeeTech which seems to be more advanced than HiWonder LewanSoul, but weaker and cheaper than the strong Korean guy Dynamixel. Not thing urgent. Will tell you more about it later. Cheers. Adrian Maire13.4kThu 22:22“so you mean you are not writing your own library but just assessing the open source library”
All code is mine, but you were looking the wrong file.The checksum is the following:

user image

The negation is with the “~”, I think it’s the same in pythonAbout Feetech, I have seen them yes, but to have decent torque (e.g. >15kg servo), they are pretty expensive (over 50$), so to me is very similar to Dynamixel. You are right that for small (<10kg) some are cheap. tlfong014137Thu 23:06@AdrianMaire Ah, sorry, I misunderstood. So you are writing your own library. Me too, I am also writing my own library in python.@AdrianMaire Thanks a lot for your checksum function. Yes, “~” in python is also bitwise not (1 to 0, and 0 to one). I see that you are using buf which is the same of that of the open source library. Python seems to prefer their “bytes” data structure, which causes me much trouble.Anyway, I got stuck in converting “bytes” to “byte list”, then to string etc, but I think I am walking the last mile, and should full debug in another 2 hours or so. I used your very first command of read ServoId and found your comment clear. Many thanks.@AdrianMaire The FeeTech servo I already ordered is the following:item.taobao.com/…FEETECH 飞特双轴 19KG 磁编码360度TTL信号电平机器人串行总线舵机 单价:99元Translation: FeeTech, double axis, 19kg, magnetic encoding (Note 1), 360 degrees, TTL level signal, serial bus servo. Note 1 – The magnetic encoding is high resolution, I forgot if 11 bit resolution or higher, anyway not using analog pot is superior than HiWonder. Other good things including the following, (1) can read current (ie. loading) , read stall status,(2) They have higher, more expensive models using RS485, CAN (I have zero experience) so the upgrade path to industrial grade project is smooth. I feel inferior always doing RS232, I hope to upgrade to RS485, of course it is a long term thing, perhaps 1 to 2 years. Good night. PS – The model I ordered is CNY99, or 99/6.28 = US$16. Adrian MaireFri 0:02@tlfong01 Buffer is just an std::array<uint8_t,10>, that is, an array of 10 char.Woo, nice servo!  22 hours later… tlfong014137Fri 22:28@AdrianMaire Ah yes, I did use “Buffer” when I played with Arduino C++ UART, some 7 or 8 years ago. So when I saw your “std::array…” I surprisingly found that I have already forgotten the meaning of the symbol “::”. I vaguely remember this “::” symbol is related to the “class inheritance” thing in OO programming. But for now I will only play with python and forget C++ altogether, at least for 2 or 3 years, I hope.My toys arrived yesterday and I collected them an hour ago.

user image

It seems that (1) the servo sends DTR signal (so are you jealous?), (2) UART speed range is 38.4kbd ~ 1000kbd, (I hope to use low speed of 38k4bs to see if the reading error drops. (3) 12 bit (4,096 resolution) magnetic position encoding (as I say, this is truly digital servo, while LX-224HY using analog potentiometer is fake digital. tlfong014137Fri 22:59

user image

It appears that we don’t need that many servos to make a cat or a dog.Cats and Dogs
(1) kickstarter.com/projects/…
(2) interestingengineering.com/…
(3) youtube.com/…. Ah bed time. See you tomorrow. Adrian Maire13.4kFri 23:35“Ah yes, I did use “Buffer” when I played with Arduino C++ UART”
hmmm, it is a custom-defined type, I doubt you used the same “Buffer”, but this is a quite generic name.“::” has several meanings, usually to access static parameters. In this case, “std” is a namespace used for all the STL (standard library of C++). And “array” is one of the data-structure of it.“LX-224HY using analog potentiometer is fake digital”
Seems amazing indeed!!
But don’t mix AD converter resolution with precision: the potentiometer will probably wear faster than your magnetic hall sensor, but magnets are subject to external influence (e.g. put a magnet close to the servo). And you don’t know the resolution of the potentiometer AD used in the Hiwonder.(time to go to the shop for next week food).  2 days later… tlfong014137Sun 16:29@AdrianMaire Buffer: Ah yes, I did use “character buff” when playing with arduino C++ UART projects. I am using python “bytes” data structure, converting to bytes list, which is a list of bytes, and then converting to hex string to print out, therefore making a big mess.Anyway, I will stall for a short while, and resume when I finish testing the FeeTech FE-URT-1, converter of UART to SMS Servo (RS485) & SCS Servo (TTL). But I don’t know nothing about RS485. So it might take me some time to get to know them.AliExpress sells FE-URT-1 at US$14. Here is the link:Feetech FE-URT-1 Multi function serial port signal converter USB/URAT to SMS Servo (RS485) &SCS Servo (TTL) US$14
aliexpress.com/item/…I bought my FE-URT-1 from TaoBao (like AliExpress, belonging to AliBaba) for 40 yuans. In case you are jealous the we can buy cheap, you must understand we workers here also got paid as cheap.飞特总线舵机USB转485.TTL信号转换板 举报 价格 ¥40
item.taobao.com/…tlfong014137Sun 17:05Now I am reading the URT1 board product features/guide and making a summary. The translated version’s English is lousy. I guess I might need to Chinese version to clarify things.Just thinking aloud. Nothing urgent. No need to reply.URT-1 Offline adaptor/interface board for Feite SMS and SCS servos reading notes:
01. Can select USB to 3V/5V signals.
02. Two kinds of signals: (a) Asynchronous half-duplex RS485 for SMS serial servo, (b) – Asynchronous half-duplex TTL-level for SCS servo.
03. URT-1 board has “hardware time multiplex automatic bypass” (not sure what it is, perhaps just full duplex to half duplex by hardware.
04. Two interfaces: (a) SMS serial servo interface using 5264-4AW, (b) SCS series servo interface using 5264-3AW interface.(see full text)tlfong014137Sun 17:20There are two versions of the board: Old and New. Mine is new. 🙂

user image

Locking down supper time! Will come back late this evening to start testing. Cheers.  1 hour later… Adrian Maire13.4kSun 18:30Sound good! 🙂
Alternatively, we could create our own servo driver. We buy the best motor/sensor we can find (at correct price), and use some cheap 2yen ARM with UART to drive it. This way we could even have Sin movements or similarsFor the 555, I need to print a small board (that means to remember all the process or etching, etc.). It’s taking to me some time.  3 hours later… tlfong014137Sun 21:56@AdrianMaire Yes, my long term (3 year) goal is to use Cortex M0 Micro Python, or BBC MicroBit micro python to drive it. But before that, perhaps we can do some sort of pair programming/development: You Cortex M4 C/C++, me Rpi4 python. My short term (1 year) goal is to build a robot cat/dog/turtle, using 4 servos.@AdrianMaire Yes, PCB etching is time consuming. For prototyping, I prefer to use bread board, then proto board. I once thought of learning Eagle PCB CAD, but the learning curve is too steep for me casual hobbyists.Anyway, I have downloaded the FeeTech URT1 Board. It took me some 15 minutes to find the correct software programs.(1) CH340C driver download address:
wch.cn/download/…
(2) FD V1.6 .exe download address:
drive.google.com/file/d/…Now I have wired the hardware, and started the FD1.6 debug program.

user image

For Win10, I need to set power switch to 5V. I first set to 3V, but then the CH340 driver was not detected. It took me 10 minutes to find that out. I use 6V to power the servo. I forgot if the max is 7.5V (no longer 11~12V for LX-224HV). But for RS485 servo, the power is 8V~24V. I hope I can use 24V for torque servos, yes higher that 12V of LX-224HV, so those HiWonder guys can not LOL at we FeeTech guys.Anyway, I started the FD1.6 debug tool, and my first impression is that compared to the HigWonder debug board targeted to perhaps 12~16 school children using BBC MicroBit, this FeeTech debug board can entertain Arduino/Rpi users well. The FD1.6 interface looks complex and I think industrial automation engineers should find this tools helpful.

user image

Looking back, I am glad your LX-224HV half duplex program introduced me to the new world of Serial Bus Servos. As I said some days ago, I feel inferior and losing face playing with TowerPro MG995 toy servos, using PWM to do the vary limited control, and open loop, no feedback from the servo to do closed loop control projects.Ah, bed time! Call it a day. Cheers.  11 hours later… tlfong01yst 9:33Question – As I said earlier, I am thinking of using M0 as the target machine. I know it is too weak to be a development machine. I know STM32 can do RS485. Do you recommend any thing like “Blue Pill” which is cheap, newbie friendly, but still can do RS485? I heard of “Black Pill” which has DSP, but I think it is too much for newbie me.  3 hours later… tlfong014137yst 12:08I found this TT-Link mini circuit from FeeTech. What do you think about it?

user image

  3 hours later… tlfong014137yst 15:03Now I am reading the URT-1 Status Byte and Commands List, and making a summary.Serial Bus Smart Control servo SCS15 Manual v1.02 – FeeTech 2016dec21
grobotronics.com/images/…

2.1 Current status byte

BIT7 0 –
BIT6 0 –
BIT5 1 – Over load
BIT4 0 –
BIT3 0 –
BIT2 1 – Over temperature
BIT1 1 –
BIT0 1 – Over/under voltage

Note : All bits = 0 means no error

2.2 Memory control table (edit v0.1 tlfong01 2020sep18hkt1456)

——————————————————————————————————-(see full text)The above text file is not well formatted. Here is a pretty print.

user image

tlfong014137yst 15:29The above pretty print image is not complete. Here is the complete one.

user image

tlfong01yst 15:40So I finished reading the SCS15 Servo’s 10 page long manual. I could not find any other manuals for higher/newer models. So I will come back the the URT-1 board and start learning how to use it. Tea time. See you later. tlfong014137yst 16:03I first tried the program mode to read and write to the memory locations and found everything OK.

user image

  5 hours later… tlfong014137yst 21:03I scratched my rusty little head for 10 minutes but still got no clue how the following TTLinker works. So I gave up. Do you have any idea?

user image

tlfong014137yst 21:17Now I am reading references on FeeTech SCS15 servo. The following article has a good overview and an Arduino demo program. Now I know SCS15 indicates 15kgcm. The most powerful analog servo I have played so far is TowerPro MG996R, which is only 10kgcm. So LX-224HV 20kg is indeed very powerful.FeeTech SC Servo SCS15 Overview and Arduino Demo Program
ecksteinimg.de/Datasheet/…tlfong014137yst 21:38What I don’t understand in the following diagram of TTLinker, why the pink bubble TxD after conversion becomes TxEn.

user image

tlfong014137yst 22:01Now I have found an Arduino library for FeeTech SCS15 Servo. The library seems small. I am going to steal the code and convert it to my own DIY python library for SCS15 servo.Arduino Library for FeeTEch SCS15 – Elecrow
elecrow.com/download/…Ah bed time! Will carry on tomorrow. Cheers. youtube.com/…  12 hours later… tlfong01413710:33FeeTech FAQ – I found that the first time setting up, I could scanned the servo ID, but no longer afterwards. I then googled FeeTech FAQ and found that there are a couple of traps that newbies don’t know that they don’t know. So I think the FD tool is not mature at all. Below is the relevant section of the FAQ.FeeTech FAQ
feetechrc.com/faq.html

– English –

5. Why FD search does not find ID?

Part 1 – Only one servo connected:

(c) For SCS15, recommend to power 7V ~ 8.4V

(e) SCS Servo default baud rate is 1Mbaud

Part 2 – More than one servo connected:

(g) Before connecting all servos, each servo must first be individually initialized using FD, with unique servo ID for each servo.

(h) Use exteral power supply with high capacity.

(i) If first search OK, but not afterwards, need to check if parameters have been modified.(see full text)  1 hour later… tlfong01413711:47I am going to test my FeeTech STS3215 Servo, using Rpi4B Thonny Python 3.7.3, with my fake TTLinker Mini (3V3 Full Duplex UART to 5V0 Half Duplex UART (3 versions)) (a) TXS0102/04 based, (b) HC03 + CD4050 based, (c) HC04 based. But at the same time, I have order the real TTLinker Mini from the TaoBao shop below. AliBaba also sells it, but minimum order is 10 pieces!(1) AliBaba Mini TTLinker Signal Conversion Board Connect eArduino to SCS15 Servo – US$1 (10 pcs min order)
alibaba.com/product-detail/…

(2) TaoBao TTLinker-mini – ¥25
item.taobao.com/…tlfong01413712:17Last time I said the SCS15 Servo Arduino Library seems small. Then I found it is not complete library, without much documentation. So I googled again, and found another one for SCS Servo 1610. The library seem more official and complete. So I forget the old library and try this new one instead.SCServoArduinoLib for SCServo1610.zip
drive.google.com/file/d/….  4 hours later… tlfong01413716:31Now I am studying the FeeTech’s official SCS Protocol Library. The original code has comments in Simplified Chinese characters. I need to load the files into my Traditional Chinese Windows 10 Word and then convert them into Win10 EditPlus in unicode. The .h and .cpp files are fully listed below.

/*
 * SCSProtocol.h
 * SCS 串行舵机协议程序 [serial servo protocol programs - tlfong01's translation from Chinese v0.1  2020sep29hkt1459]
 * 日期: 2016.8.25
 * 作者: 谭雄乐 [Tam Hung Lok]
 */

#ifndef _SCSPROTOCOL_H
#define _SCSPROTOCOL_H

typedef		char			s8;
typedef		unsigned char	u8;
typedef		unsigned short	u16;
typedef		short			s16;
typedef		unsigned long	u32;
typedef		long			s32;

class SCSProtocol{
public:
	SCSProtocol();
	int genWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen);//普通写指令 [general write instruction

(see full text)

/*
 * SCSProtocol.cpp
 * SCS串行舵机协议程序 [scs serial servo portocol program]
 * 日期: 2016.8.25
 * 作者: 谭雄乐 [Tam Hung Lok]
 */

#include <stddef.h>
#include "SCSProtocol.h"

SCSProtocol::SCSProtocol()
{
	Level = 1;//除广播指令所有指令返回应答 [except broadcast instruction, all instructions return reply]
	End = 1;//舵机处理器与控制板处理器端结构不一致 [servo processor is different from control boardside structure not identical]
}

//1个16位数拆分为2个8位数 [1 16-bit integer split into 2 8-bit integer]
//DataL为低位,DataH为高位 [DataL is low, DataH is High]
void SCSProtocol::Host2SCS(u8 *DataL, u8* DataH, int Data)
{
	if(End){
		*DataL = (Data>>8);
		*DataH = (Data&0xff);
	}else{
		*DataH = (Data>>8);
		*DataL = (Data&0xff);
	}
}

//2个8位数组合为1个16位数 [2 8-bit numbers combined into 1 16-bit integer]
//DataL为低位,DataH为高位 [DataL is low, DataH is high]
int SCSProtocol::SCS2Host(u8 DataL, u8 DataH)
{
	int Data;
	if(End){
		Data = DataL;
		Data<<=8;
		Data |= DataH;
	}else{
		Data = DataH;
		Data<<=8;
		Data |= DataL;
	}
	return Data;
}

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);
}

//普通写指令 [general servo instruction]
//舵机ID,MemAddr内存表地址,写入数据,写入长度 [Servo ID, MemAddr RAM Control table address, write data, write length]
int SCSProtocol::genWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen)
{
	flushSCS();
	writeBuf(ID, MemAddr, nDat, nLen, INST_WRITE);
	return Ack(ID);
}

//异步写指令 [async write instruction]
//舵机ID,MemAddr内存表地址,写入数据,写入长度 [Servo ID, MemAddr RAM Control table address, write data, write length]
int SCSProtocol::regWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen)
{
	flushSCS();
	writeBuf(ID, MemAddr, nDat, nLen, INST_REG_WRITE);
	return Ack(ID);
}

//同步写指令 [sync write instruction]
//舵机ID[]数组,IDN数组长度,MemAddr内存表地址,写入数据,写入长度 [Servo ID [], MemAddr RAM Control table address, write data, write length]
void SCSProtocol::snycWrite(u8 ID[], u8 IDN, u8 MemAddr, u8 *nDat, u8 nLen)
{
	u8 mesLen = ((nLen+1)*IDN+4);
	u8 Sum = 0;
	u8 bBuf[7];
	bBuf[0] = 0xff;
	bBuf[1] = 0xff;
	bBuf[2] = 0xfe;
	bBuf[3] = mesLen;
	bBuf[4] = INST_SYNC_WRITE;
	bBuf[5] = MemAddr;
	bBuf[6] = nLen;
	writeSCS(bBuf, 7);

	Sum = 0xfe + mesLen + INST_SYNC_WRITE + MemAddr + nLen;
	u8 i, j;
	for(i=0; i<IDN; i++){
		writeSCS(ID[i]);
		writeSCS(nDat, nLen);
		Sum += ID[i];
		for(j=0; j<nLen; j++){
			Sum += nDat[j];
		}
	}
	writeSCS(~Sum);
}

int SCSProtocol::writeByte(u8 ID, u8 MemAddr, u8 bDat)
{
	flushSCS();
	writeBuf(ID, MemAddr, &bDat, 1, INST_WRITE);
	return Ack(ID);
}

int SCSProtocol::writeWord(u8 ID, u8 MemAddr, u16 wDat)
{
	flushSCS();
	u8 buf[2];
	Host2SCS(buf+0, buf+1, wDat);
	writeBuf(ID, MemAddr, buf, 2, INST_WRITE);
	return Ack(ID);
}

int SCSProtocol::EnableTorque(u8 ID, u8 Enable)
{
	return writeByte(ID, P_TORQUE_ENABLE, Enable);
}

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);
}

//写位置指令 [write position instruction]
//舵机ID,Position位置,执行时间Time,执行速度Speed [servo ID, position, execution time, execution speed]
int SCSProtocol::WritePos(u8 ID, u16 Position, u16 Time, u16 Speed)
{
	return writePos(ID, Position, Time, Speed, INST_WRITE);
}

//异步写位置指令 [async write position instruction]
//舵机ID,Position位置,执行时间Time,执行速度Speed  [servo ID, position, execution time, execution speed]
int SCSProtocol::RegWritePos(u8 ID, u16 Position, u16 Time, u16 Speed)
{
	return writePos(ID, Position, Time, Speed, INST_REG_WRITE);
}

void SCSProtocol::RegWriteAction()
{
	writeBuf(0xfe, 0, NULL, 0, INST_ACTION);
}

//写位置指令 [write position instruction]
//舵机ID[]数组,IDN数组长度,Position位置,执行时间Time,执行速度Speed [servo ID list [], IDN list length, exe time, exe speed]
void SCSProtocol::SyncWritePos(u8 ID[], u8 IDN, u16 Position, u16 Time, u16 Speed)
{
	u8 buf[6];
	Host2SCS(buf+0, buf+1, Position);
	Host2SCS(buf+2, buf+3, Time);
	Host2SCS(buf+4, buf+5, Speed);
	snycWrite(ID, IDN, P_GOAL_POSITION_L, buf, 6);
}

//读指令 [read instruction]
//舵机ID,MemAddr内存表地址,返回数据nData,数据长度nLen [servo ID, MemAddr RAM control table addr, return data nData, data nLen]
int SCSProtocol::Read(u8 ID, u8 MemAddr, u8 *nData, u8 nLen)
{
	flushSCS();
	writeBuf(ID, MemAddr, &nLen, 1, INST_READ);
	u8 bBuf[5];
	if(readSCS(bBuf, 5)!=5){
		return 0;
	}
	int Size = readSCS(nData, nLen);
	if(readSCS(bBuf, 1)){
		return Size;
	}
	return 0;
}

//读1字节,超时返回-1 [read 1 byte, timeout return -1]
int SCSProtocol::readByte(u8 ID, u8 MemAddr)
{
	u8 bDat;
	int Size = Read(ID, MemAddr, &bDat, 1);
	if(Size!=1){
		return -1;
	}else{
		return bDat;
	}
}

//读2字节,超时返回-1 [read two bytes, timeout return -1]
int SCSProtocol::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]);
	return wDat;
}

//读位置,超时返回-1 [read position, timeout return -1]
int SCSProtocol::ReadPos(u8 ID)
{
	return readWord(ID, P_PRESENT_POSITION_L);
}

//多圈控制指令 [multiple loop control instruction]
int SCSProtocol::WriteSpe(u8 ID, s16 Speed)
{
	if(Speed<0){
		Speed = -Speed;
		Speed |= (1<<10);
	}
	return writeWord(ID, P_GOAL_TIME_L, Speed);
}

//读电压,超时返回-1 [read voltage in, timeout return -1]
int SCSProtocol::ReadVoltage(u8 ID)
{	
	return readByte(ID, P_PRESENT_VOLTAGE);
}

//读温度,超时返回-1 [read timperatur, timeout return -1]
int SCSProtocol::ReadTemper(u8 ID)
{	
	return readByte(ID, P_PRESENT_TEMPERATURE);
}

//Ping指令,返回舵机ID,超时返回-1 [ping instruction, return servo ID, timeout return -1]
int SCSProtocol::Ping(u8 ID)
{
	flushSCS();
	u8 bBuf[6];
	writeBuf(ID, 0, NULL, 0, INST_PING);
	int Size = readSCS(bBuf, 6);
	if(Size==6){
		return bBuf[2];
	}else{
		return -1;
	}
}

int SCSProtocol::wheelMode(u8 ID)
{
	u8 bBuf[4];
	bBuf[0] = 0;
	bBuf[1] = 0;
	bBuf[2] = 0;
	bBuf[3] = 0;
	return genWrite(ID, P_MIN_ANGLE_LIMIT_L, bBuf, 4);
}

int SCSProtocol::joinMode(u8 ID, u16 minAngle, u16 maxAngle)
{
	u8 bBuf[4];
	Host2SCS(bBuf, bBuf+1, minAngle);
	Host2SCS(bBuf+2, bBuf+3, maxAngle);
	return genWrite(ID, P_MIN_ANGLE_LIMIT_L, bBuf, 4);
}

//复位舵机参数为默认值 [rest servo parameters to default values]
int SCSProtocol::Reset(u8 ID)
{
	flushSCS();
	writeBuf(ID, 0, NULL, 0, INST_RESET);
	return Ack(ID);
}

int	SCSProtocol::Ack(u8 ID)
{
	if(ID != 0xfe && Level){
		u8 buf[6];
		u8 Size = readSCS(buf, 6);
		if(Size!=6){
			return 0;
		}
	}
	return 1;
}

The .cpp file is only 306 lines long. I don’t fully understand all the terms used (eg async write vs general write), so my translation from Chinese to English might be wrong or misleading. Anyway, I will now write a minimal python program to test my fake TTLing Mini board (TXS0104 first, then HC04 + CD4050, theh HX04). 

Categories: Uncategorized

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out /  Change )

Google photo

You are commenting using your Google account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s

This site uses Akismet to reduce spam. Learn how your comment data is processed.