优化了平衡，尤其对于向后倾倒的几种极限情况进行优化

Shibo-main-TOMMY/include/Servo_STS3032.h

@@ -0,0 +1,108 @@
+// -----------------------------------------------------------------------------
+// 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舵机同步控制指令
+// 调用方式为sms_sts.SyncWritePosEx(ID, 2, Position, Speed, ACC);
+
+#ifndef _SERVO_STS3032_H
+#define _SERVO_STS3032_H
+
+#if defined(ARDUINO) && ARDUINO >= 100
+#include "Arduino.h"
+#else
+#include "WProgram.h"
+#endif
+
+// 数据结构定义
+typedef char s8;
+typedef unsigned char u8;
+typedef unsigned short u16;
+typedef short s16;
+typedef unsigned long u32;
+typedef long s32;
+
+// 内存表定义
+#define SMS_STS_ACC 41
+#define INST_SYNC_WRITE 0x83
+
+// 飞特串行舵机通信层协议程序
+class SCS
+{
+public:
+	SCS();
+	SCS(u8 End);
+	SCS(u8 End, u8 Level);
+	void syncWrite(u8 ID[], u8 IDN, u8 MemAddr, u8 *nDat, u8 nLen); // 同步写指令
+public:
+	u8 Level; // 舵机返回等级
+	u8 End;	  // 处理器大小端结构
+	u8 Error; // 舵机状态
+	u8 syncReadRxPacketIndex;
+	u8 syncReadRxPacketLen;
+	u8 *syncReadRxPacket;
+	u8 *syncReadRxBuff;
+	u16 syncReadRxBuffLen;
+	u16 syncReadRxBuffMax;
+	u32 syncTimeOut;
+
+protected:
+	virtual int writeSCS(unsigned char *nDat, int nLen) = 0;
+	virtual int writeSCS(unsigned char bDat) = 0;
+	virtual void rFlushSCS() = 0;
+	virtual void wFlushSCS() = 0;
+
+protected:
+	void Host2SCS(u8 *DataL, u8 *DataH, u16 Data); // 1个16位数拆分为2个8位数
+	int Ack(u8 ID);								   // 返回应答
+	int checkHead();							   // 帧头检测
+};
+
+// 飞特串行舵机硬件接口层程序
+class SCSerial : public SCS
+{
+public:
+	SCSerial();
+	SCSerial(u8 End);
+	SCSerial(u8 End, u8 Level);
+
+protected:
+	int writeSCS(unsigned char *nDat, int nLen); // 输出nLen字节
+	int writeSCS(unsigned char bDat);			 // 输出1字节
+	void rFlushSCS();							 //
+	void wFlushSCS();							 //
+public:
+	unsigned long IOTimeOut; // 输入输出超时
+	HardwareSerial *pSerial; // 串口指针
+	int Err;
+
+public:
+	virtual int getErr() { return Err; }
+};
+
+// 飞特SMS/STS系列串行舵机应用层程序
+class SMS_STS : public SCSerial
+{
+public:
+	virtual void SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[]); // 同步写多个舵机位置指令
+};
+
+#endif