switched the servo driver to FTServo_Arduino

Author: Forairaaaaa

src/M5StackChan.cpp

@@ -5,7 +5,7 @@
  */
 #include "M5StackChan.h"
 #include "drivers/PY32IOExpander/PY32IOExpander.hpp"
-#include "drivers/SCServo_lib/src/SCSCL.h"
+#include "drivers/FTServo_Arduino/src/SCSCL.h"
 #include "utils/compat/make_unique.h"
 #include "utils/settings/settings.h"
 #include "utility/power/INA226_Class.hpp"

src/drivers/FTServo_Arduino/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2024 ftservo
+
+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.

src/drivers/FTServo_Arduino/README.md

@@ -0,0 +1,44 @@
+# FEETECH BUS Servo
+
+> FEETECH BUS Servo library for Arduino and ESP32
+
+## Table of Contents
+
+<!-- TOC -->
+* [FEETECH BUS Servo](#ft-series-servo)
+  * [Table of Contents](#table-of-contents)
+  * [Requirements](#requirements)
+  * [Usage](#usage)
+  * [Notes](#notes)
+  * [Release](#release)
+<!-- TOC -->
+
+## Requirements
+
+* arduino-1.6.10 or newer(https://www.arduino.cc/).
+
+## Usage
+
+For usage examples, see the [examples](./examples) directory.
+
+## Notes
+
+The code for the `SCServo` library is divided into the following
+parts:
+
+* communication layer: [src/SCS.cpp](src/SCS.cpp)
+* hardware interface layer: [src/SCSerial.cpp](src/SCSerial.cpp)
+* application layer:
+  * corresponds to the three series of FEETECH BUS Servo
+    * `SCSCL` application layer program: [src/SCSCL.h](src/SCSCL.h) and [src/SCSCL.cpp](src/SCSCL.cpp)
+    * `SMS` and `STS` application layer program: [src/SMS_STS.h](src/SMS_STS.h) and [src/SMS_STS.cpp](src/SMS_STS.cpp)
+    * `HLS` application layer program: [src/HLSCL.h](src/HLSCL.h) and [src/HLSCL.cpp](src/HLSCL.cpp)
+* instruction definition header file: [src/INST.h](src/INST.h)
+* communication layer program: [src/SCS.h](src/SCS.h) and [src/SCS.cpp](src/SCS.cpp)
+* hardware interface program: [src/SCSerial.h](src/SCSerial.h) and [src/SCSerial.cpp](src/SCSerial.cpp)
+
+Note that there are differences in the memory table definitions of different series of FEETECH BUS servos.
+
+## Release
+
+New (GitHub) releases of this library are automatically ingressed by the Arduino Library Manager.

src/drivers/FTServo_Arduino/library.properties

@@ -0,0 +1,11 @@
+# see https://arduino.github.io/arduino-cli/1.0/library-specification/#library-metadata
+
+name=FTServo
+version=2.0.0
+author=ftservo
+maintainer=FEETECH <github.com/ftservo>
+sentence=FEETECH BUS Servo library for Arduino and ESP32
+paragraph=This library is compatible with all series of FEETECH BUS Servo
+category=Other
+url=https://github.com/ftservo/FTServo_Arduino
+architectures=*

src/drivers/FTServo_Arduino/src/HLSCL.cpp

@@ -0,0 +1,260 @@
+/*
+ * HLSCL.cpp
+ * 飞特HTS/HLS系列串行舵机应用层程序
+ * 日期: 2024.11.21
+ * 作者: txl
+ */
+
+#include "HLSCL.h"
+
+HLSCL::HLSCL()
+{
+	End = 0;
+}
+
+HLSCL::HLSCL(u8 End):SCSerial(End)
+{
+}
+
+HLSCL::HLSCL(u8 End, u8 Level):SCSerial(End, Level)
+{
+}
+
+int HLSCL::WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC, u16 Torque)
+{
+	if(Position<0){
+		Position = -Position;
+		Position |= (1<<15);
+	}
+	u8 bBuf[7];
+	bBuf[0] = ACC;
+	Host2SCS(bBuf+1, bBuf+2, Position);
+	Host2SCS(bBuf+3, bBuf+4, Torque);
+	Host2SCS(bBuf+5, bBuf+6, Speed);
+	
+	return genWrite(ID, HLSCL_ACC, bBuf, 7);
+}
+
+int HLSCL::RegWritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC, u16 Torque)
+{
+	if(Position<0){
+		Position = -Position;
+		Position |= (1<<15);
+	}
+	u8 bBuf[7];
+	bBuf[0] = ACC;
+	Host2SCS(bBuf+1, bBuf+2, Position);
+	Host2SCS(bBuf+3, bBuf+4, Torque);
+	Host2SCS(bBuf+5, bBuf+6, Speed);
+	
+	return regWrite(ID, HLSCL_ACC, bBuf, 7);
+}
+
+void HLSCL::SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[], u16 Torque[])
+{
+    u8 offbuf[7*IDN];
+    for(u8 i = 0; i<IDN; i++){
+		if(Position[i]<0){
+			Position[i] = -Position[i];
+			Position[i] |= (1<<15);
+		}
+		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, Torque[i]);
+        Host2SCS(offbuf+i*7+5, offbuf+i*7+6, Speed[i]);
+    }
+    syncWrite(ID, IDN, HLSCL_ACC, offbuf, 7);
+}
+
+void HLSCL::SyncWriteSpe(u8 ID[], u8 IDN, s16 Speed[], u8 ACC[], u16 Torque[])
+{
+    u8 offbuf[7*IDN];
+    for(u8 i = 0; i<IDN; i++){
+		if(Speed[i]<0){
+			Speed[i] = -Speed[i];
+			Speed[i] |= (1<<15);
+		}
+		if(ACC){
+			offbuf[i*7] = ACC[i];
+		}else{
+			offbuf[i*7] = 0;
+		}
+        Host2SCS(offbuf+i*7+1, offbuf+i*7+2, 0);
+        Host2SCS(offbuf+i*7+3, offbuf+i*7+4, Torque[i]);
+        Host2SCS(offbuf+i*7+5, offbuf+i*7+6, Speed[i]);
+    }
+    syncWrite(ID, IDN, HLSCL_ACC, offbuf, 7);
+}
+
+int HLSCL::WheelMode(u8 ID)
+{
+	return writeByte(ID, HLSCL_MODE, 1);		
+}
+
+int HLSCL::EleMode(u8 ID)
+{
+	return writeByte(ID, HLSCL_MODE, 2);		
+}
+
+int HLSCL::WriteSpe(u8 ID, s16 Speed, u8 ACC, u16 Torque)
+{
+	if(Speed<0){
+		Speed = -Speed;
+		Speed |= (1<<15);
+	}
+	u8 bBuf[7];
+	bBuf[0] = ACC;
+	Host2SCS(bBuf+1, bBuf+2, 0);
+	Host2SCS(bBuf+3, bBuf+4, Torque);
+	Host2SCS(bBuf+5, bBuf+6, Speed);
+	
+	return genWrite(ID, HLSCL_ACC, bBuf, 7);
+}
+
+int HLSCL::WriteEle(u8 ID, s16 Torque)
+{
+	if(Torque<0){
+		Torque = -Torque;
+		Torque |= (1<<15);
+	}
+	return writeWord(ID, HLSCL_GOAL_TORQUE_L, Torque);
+}
+
+int HLSCL::EnableTorque(u8 ID, u8 Enable)
+{
+	return writeByte(ID, HLSCL_TORQUE_ENABLE, Enable);
+}
+
+int HLSCL::unLockEprom(u8 ID)
+{
+	EnableTorque(ID, 0);
+	return writeByte(ID, HLSCL_LOCK, 0);
+}
+
+int HLSCL::LockEprom(u8 ID)
+{
+	return writeByte(ID, HLSCL_LOCK, 1);
+}
+
+int HLSCL::CalibrationOfs(u8 ID)
+{
+	EnableTorque(ID, 0);
+	unLockEprom(ID);
+	return Recal(ID);
+}
+
+int HLSCL::FeedBack(int ID)
+{
+	int nLen = Read(ID, HLSCL_PRESENT_POSITION_L, Mem, sizeof(Mem));
+	if(nLen!=sizeof(Mem)){
+		return -1;
+	}
+	return nLen;
+}
+
+int HLSCL::ReadPos(int ID)
+{
+	int Pos = -1;
+	if(ID==-1){
+		Pos = Mem[HLSCL_PRESENT_POSITION_H-HLSCL_PRESENT_POSITION_L];
+		Pos <<= 8;
+		Pos |= Mem[HLSCL_PRESENT_POSITION_L-HLSCL_PRESENT_POSITION_L];
+	}else{
+		Pos = readWord(ID, HLSCL_PRESENT_POSITION_L);
+	}
+	if(Pos&(1<<15)){
+		Pos = -(Pos&~(1<<15));
+	}
+	
+	return Pos;
+}
+
+int HLSCL::ReadSpeed(int ID)
+{
+	int Speed = -1;
+	if(ID==-1){
+		Speed = Mem[HLSCL_PRESENT_SPEED_H-HLSCL_PRESENT_POSITION_L];
+		Speed <<= 8;
+		Speed |= Mem[HLSCL_PRESENT_SPEED_L-HLSCL_PRESENT_POSITION_L];
+	}else{
+		Speed = readWord(ID, HLSCL_PRESENT_SPEED_L);
+	}
+	if(Speed&(1<<15)){
+		Speed = -(Speed&~(1<<15));
+	}	
+	return Speed;
+}
+
+int HLSCL::ReadLoad(int ID)
+{
+	int Load = -1;
+	if(ID==-1){
+		Load = Mem[HLSCL_PRESENT_LOAD_H-HLSCL_PRESENT_POSITION_L];
+		Load <<= 8;
+		Load |= Mem[HLSCL_PRESENT_LOAD_L-HLSCL_PRESENT_POSITION_L];
+	}else{
+		Load = readWord(ID, HLSCL_PRESENT_LOAD_L);
+	}
+	if(Load&(1<<10)){
+		Load = -(Load&~(1<<10));
+	}
+	return Load;
+}
+
+int HLSCL::ReadVoltage(int ID)
+{	
+	int Voltage = -1;
+	if(ID==-1){
+		Voltage = Mem[HLSCL_PRESENT_VOLTAGE-HLSCL_PRESENT_POSITION_L];	
+	}else{
+		Voltage = readByte(ID, HLSCL_PRESENT_VOLTAGE);
+	}
+	return Voltage;
+}
+
+int HLSCL::ReadTemper(int ID)
+{	
+	int Temper = -1;
+	if(ID==-1){
+		Temper = Mem[HLSCL_PRESENT_TEMPERATURE-HLSCL_PRESENT_POSITION_L];	
+	}else{
+		Temper = readByte(ID, HLSCL_PRESENT_TEMPERATURE);
+	}
+	return Temper;
+}
+
+int HLSCL::ReadMove(int ID)
+{
+	int Move = -1;
+	if(ID==-1){
+		Move = Mem[HLSCL_MOVING-HLSCL_PRESENT_POSITION_L];	
+	}else{
+		Move = readByte(ID, HLSCL_MOVING);
+	}
+	return Move;
+}
+
+int HLSCL::ReadCurrent(int ID)
+{
+	int Current = -1;
+	if(ID==-1){
+		Current = Mem[HLSCL_PRESENT_CURRENT_H-HLSCL_PRESENT_POSITION_L];
+		Current <<= 8;
+		Current |= Mem[HLSCL_PRESENT_CURRENT_L-HLSCL_PRESENT_POSITION_L];
+	}else{
+		Current = readWord(ID, HLSCL_PRESENT_CURRENT_L);
+	}
+	if(Current&(1<<15)){
+		Current = -(Current&~(1<<15));
+	}	
+	return Current;
+}
+
+int HLSCL::ServoMode(u8 ID)
+{
+	return writeByte(ID, HLSCL_MODE, 0);		
+}