Open source demo.

3.Software/wl_pro_robot/Servo_STS3032.cpp

@@ -0,0 +1,150 @@
+// -----------------------------------------------------------------------------
+// Copyright (c) 2024 Mu Shibo
+// 
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+// 
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+// 
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+// -----------------------------------------------------------------------------
+
+//飞特STS3032舵机同步控制指令
+
+#include "Servo_STS3032.h"
+#include <stddef.h>
+
+//飞特串行舵机通信层协议程序
+SCS::SCS()
+{
+	Level = 1;//除广播指令所有指令返回应答
+	Error = 0;
+}
+SCS::SCS(u8 End)
+{
+	Level = 1;
+	this->End = End;
+	Error = 0;
+}
+SCS::SCS(u8 End, u8 Level)
+{
+	this->Level = Level;
+	this->End = End;
+	Error = 0;
+}
+//1个16位数拆分为2个8位数
+//DataL为低位，DataH为高位
+void SCS::Host2SCS(u8 *DataL, u8* DataH, u16 Data)
+{
+	if(End){
+		*DataL = (Data>>8);
+		*DataH = (Data&0xff);
+	}else{
+		*DataH = (Data>>8);
+		*DataL = (Data&0xff);
+	}
+}
+//同步写指令
+//舵机ID[]数组，IDN数组长度，MemAddr内存表地址，写入数据，写入长度
+void SCS::syncWrite(u8 ID[], u8 IDN, u8 MemAddr, u8 *nDat, u8 nLen)
+{
+	rFlushSCS();
+	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+i*nLen, nLen);
+		Sum += ID[i];
+		for(j=0; j<nLen; j++){
+			Sum += nDat[i*nLen+j];
+		}
+	}
+	writeSCS(~Sum);
+	wFlushSCS();
+}
+
+//飞特串行舵机硬件接口层程序
+SCSerial::SCSerial()
+{
+	IOTimeOut = 10;
+	pSerial = NULL;
+}
+SCSerial::SCSerial(u8 End):SCS(End)
+{
+	IOTimeOut = 10;
+	pSerial = NULL;
+}
+SCSerial::SCSerial(u8 End, u8 Level):SCS(End, Level)
+{
+	IOTimeOut = 10;
+	pSerial = NULL;
+}
+int SCSerial::writeSCS(unsigned char *nDat, int nLen)
+{
+	if(nDat==NULL){
+		return 0;
+	}
+	return pSerial->write(nDat, nLen);
+}
+int SCSerial::writeSCS(unsigned char bDat)
+{
+	return pSerial->write(&bDat, 1);
+}
+void SCSerial::rFlushSCS()
+{
+	while(pSerial->read()!=-1);
+}
+void SCSerial::wFlushSCS()
+{
+}
+
+//飞特SMS_STS系列串行舵机应用层程序
+void SMS_STS::SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[])
+{
+    u8 offbuf[7*IDN];
+    for(u8 i = 0; i<IDN; i++){
+		if(Position[i]<0){
+			Position[i] = -Position[i];
+			Position[i] |= (1<<15);
+		}
+		u16 V;
+		if(Speed){
+			V = Speed[i];
+		}else{
+			V = 0;
+		}
+		if(ACC){
+			offbuf[i*7] = ACC[i];
+		}else{
+			offbuf[i*7] = 0;
+		}
+        Host2SCS(offbuf+i*7+1, offbuf+i*7+2, Position[i]);
+        Host2SCS(offbuf+i*7+3, offbuf+i*7+4, 0);
+        Host2SCS(offbuf+i*7+5, offbuf+i*7+6, V);
+    }
+    syncWrite(ID, IDN, SMS_STS_ACC, offbuf, 7);
+}
+