mirror of
https://github.com/m5stack/StackChan.git
synced 2026-05-20 11:49:45 -07:00
Merge pull request #59 from Forairaaaaa/update-firmware-v1.4.0
update firmware v1.4.0
This commit is contained in:
Forairaaaaa
@@ -2,7 +2,7 @@
|
||||
# CMakeLists in this exact order for cmake to work correctly
|
||||
cmake_minimum_required(VERSION 3.16)
|
||||
|
||||
set(PROJECT_VER "1.3.1")
|
||||
set(PROJECT_VER "1.4.0")
|
||||
add_definitions(-DFIRMWARE_VERSION=\"${PROJECT_VER}\")
|
||||
|
||||
# Add this line to disable the specific warning
|
||||
|
||||
@@ -86,6 +86,7 @@ void AppAvatar::onOpen()
|
||||
// Create default avatar
|
||||
auto avatar = std::make_unique<avatar::DefaultAvatar>();
|
||||
avatar->init(lv_screen_active());
|
||||
avatar->getPanel()->onClick().connect([&]() { _screen_clicked_flag = true; });
|
||||
GetStackChan().attachAvatar(std::move(avatar));
|
||||
|
||||
/* ------------------------------- BLE events ------------------------------- */
|
||||
@@ -205,7 +206,10 @@ void AppAvatar::onOpen()
|
||||
LvglLockGuard lvgl_lock;
|
||||
auto sequence = stackchan::animation::parse_sequence_from_json(data.data());
|
||||
if (!sequence.empty()) {
|
||||
GetStackChan().addModifier(std::make_unique<DanceModifier>(sequence));
|
||||
if (_dance_modifier_id >= 0) {
|
||||
GetStackChan().removeModifier(_dance_modifier_id);
|
||||
}
|
||||
_dance_modifier_id = GetStackChan().addModifier(std::make_unique<DanceModifier>(sequence));
|
||||
}
|
||||
});
|
||||
|
||||
@@ -242,6 +246,15 @@ void AppAvatar::onRunning()
|
||||
_ble_motion_data.data_ptr = nullptr;
|
||||
}
|
||||
|
||||
if (_screen_clicked_flag) {
|
||||
_screen_clicked_flag = false;
|
||||
if (_dance_modifier_id >= 0) {
|
||||
GetStackChan().removeModifier(_dance_modifier_id);
|
||||
_dance_modifier_id = -1;
|
||||
mclog::tagInfo(getAppInfo().name, "dance modifier removed");
|
||||
}
|
||||
}
|
||||
|
||||
GetStackChan().update();
|
||||
|
||||
view::update_home_indicator();
|
||||
|
||||
@@ -41,5 +41,8 @@ private:
|
||||
|
||||
std::unique_ptr<view::VideoWindow> _video_window;
|
||||
|
||||
bool _screen_clicked_flag = false;
|
||||
int _dance_modifier_id = -1;
|
||||
|
||||
void check_auto_angle_sync_mode();
|
||||
};
|
||||
|
||||
@@ -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.
|
||||
@@ -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.
|
||||
@@ -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=*
|
||||
@@ -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);
|
||||
}
|
||||
@@ -0,0 +1,89 @@
|
||||
/*
|
||||
* HLSCL.h
|
||||
* 飞特HLS系列串行舵机应用层程序
|
||||
* 日期: 2024.11.21
|
||||
* 作者: txl
|
||||
*/
|
||||
|
||||
#ifndef _HLSCL_H
|
||||
#define _HLSCL_H
|
||||
|
||||
//内存表定义
|
||||
//-------EPROM(只读)--------
|
||||
#define HLSCL_MODEL_L 3
|
||||
#define HLSCL_MODEL_H 4
|
||||
|
||||
//-------EPROM(读写)--------
|
||||
#define HLSCL_ID 5
|
||||
#define HLSCL_BAUD_RATE 6
|
||||
#define HLSCL_SECOND_ID 7
|
||||
#define HLSCL_MIN_ANGLE_LIMIT_L 9
|
||||
#define HLSCL_MIN_ANGLE_LIMIT_H 10
|
||||
#define HLSCL_MAX_ANGLE_LIMIT_L 11
|
||||
#define HLSCL_MAX_ANGLE_LIMIT_H 12
|
||||
#define HLSCL_CW_DEAD 26
|
||||
#define HLSCL_CCW_DEAD 27
|
||||
#define HLSCL_OFS_L 31
|
||||
#define HLSCL_OFS_H 32
|
||||
#define HLSCL_MODE 33
|
||||
|
||||
//-------SRAM(读写)--------
|
||||
#define HLSCL_TORQUE_ENABLE 40
|
||||
#define HLSCL_ACC 41
|
||||
#define HLSCL_GOAL_POSITION_L 42
|
||||
#define HLSCL_GOAL_POSITION_H 43
|
||||
#define HLSCL_GOAL_TORQUE_L 44
|
||||
#define HLSCL_GOAL_TORQUE_H 45
|
||||
#define HLSCL_GOAL_SPEED_L 46
|
||||
#define HLSCL_GOAL_SPEED_H 47
|
||||
#define HLSCL_TORQUE_LIMIT_L 48
|
||||
#define HLSCL_TORQUE_LIMIT_H 49
|
||||
#define HLSCL_LOCK 55
|
||||
|
||||
//-------SRAM(只读)--------
|
||||
#define HLSCL_PRESENT_POSITION_L 56
|
||||
#define HLSCL_PRESENT_POSITION_H 57
|
||||
#define HLSCL_PRESENT_SPEED_L 58
|
||||
#define HLSCL_PRESENT_SPEED_H 59
|
||||
#define HLSCL_PRESENT_LOAD_L 60
|
||||
#define HLSCL_PRESENT_LOAD_H 61
|
||||
#define HLSCL_PRESENT_VOLTAGE 62
|
||||
#define HLSCL_PRESENT_TEMPERATURE 63
|
||||
#define HLSCL_MOVING 66
|
||||
#define HLSCL_PRESENT_CURRENT_L 69
|
||||
#define HLSCL_PRESENT_CURRENT_H 70
|
||||
|
||||
#include "SCSerial.h"
|
||||
|
||||
class HLSCL : public SCSerial
|
||||
{
|
||||
public:
|
||||
HLSCL();
|
||||
HLSCL(u8 End);
|
||||
HLSCL(u8 End, u8 Level);
|
||||
int WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0, u16 Torque = 0);//普通写单个舵机位置指令
|
||||
int RegWritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0, u16 Torque = 0);//异步写单个舵机位置指令(RegWriteAction生效)
|
||||
void SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[], u16 Torque[]);//同步写多个舵机位置指令
|
||||
void SyncWriteSpe(u8 ID[], u8 IDN, s16 Speed[], u8 ACC[], u16 Torque[]);//同步写多个舵机速度指令
|
||||
int ServoMode(u8 ID);//Servo模式
|
||||
int WheelMode(u8 ID);//恒速模式
|
||||
int EleMode(u8 ID);//恒力模式
|
||||
int WriteSpe(u8 ID, s16 Speed, u8 ACC = 0, u16 Torque = 0);//恒速模式控制指令
|
||||
int WriteEle(u8 ID, s16 Torque);//恒力模式控制指令
|
||||
int EnableTorque(u8 ID, u8 Enable);//扭力控制指令
|
||||
int unLockEprom(u8 ID);//eprom解锁
|
||||
int LockEprom(u8 ID);//eprom加锁
|
||||
int CalibrationOfs(u8 ID);//中位校准
|
||||
int FeedBack(int ID);//反馈舵机信息
|
||||
int ReadPos(int ID);//读位置
|
||||
int ReadSpeed(int ID);//读速度
|
||||
int ReadLoad(int ID);//读输出至电机的电压百分比(0~1000)
|
||||
int ReadVoltage(int ID);//读电压
|
||||
int ReadTemper(int ID);//读温度
|
||||
int ReadMove(int ID);//读移动状态
|
||||
int ReadCurrent(int ID);//读电流
|
||||
private:
|
||||
u8 Mem[HLSCL_PRESENT_CURRENT_H-HLSCL_PRESENT_POSITION_L+1];
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,51 @@
|
||||
/*
|
||||
* INST.h
|
||||
* 飞特串行舵机协议指令定义
|
||||
* 日期: 2024.11.24
|
||||
* 作者: txl
|
||||
*/
|
||||
|
||||
#ifndef _INST_H
|
||||
#define _INST_H
|
||||
|
||||
typedef char s8;
|
||||
typedef unsigned char u8;
|
||||
typedef unsigned short u16;
|
||||
typedef short s16;
|
||||
typedef unsigned long u32;
|
||||
typedef long s32;
|
||||
|
||||
enum SCS_ERR_LIST
|
||||
{
|
||||
ERR_NO_REPLY = 1,
|
||||
ERR_CRC_CMP = 2,
|
||||
ERR_SLAVE_ID = 3,
|
||||
ERR_BUFF_LEN = 4,
|
||||
};
|
||||
|
||||
#define INST_PING 0x01
|
||||
#define INST_READ 0x02
|
||||
#define INST_WRITE 0x03
|
||||
#define INST_REG_WRITE 0x04
|
||||
#define INST_REG_ACTION 0x05
|
||||
#define INST_SYNC_READ 0x82
|
||||
#define INST_SYNC_WRITE 0x83
|
||||
#define INST_RECOVERY 0x06
|
||||
#define INST_RESET 0x0A
|
||||
#define INST_CAL 0x0B
|
||||
|
||||
//波特率定义
|
||||
#define _1M 0
|
||||
#define _0_5M 1
|
||||
#define _250K 2
|
||||
#define _128K 3
|
||||
#define _115200 4
|
||||
#define _76800 5
|
||||
#define _57600 6
|
||||
#define _38400 7
|
||||
#define _19200 8
|
||||
#define _14400 9
|
||||
#define _9600 10
|
||||
#define _4800 11
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,450 @@
|
||||
/*
|
||||
* SCS.cpp
|
||||
* 飞特串行舵机通信层协议程序
|
||||
* 日期: 2024.12.21
|
||||
* 作者: txl
|
||||
*/
|
||||
|
||||
#include <stddef.h>
|
||||
#include "SCS.h"
|
||||
|
||||
SCS::SCS()
|
||||
{
|
||||
Level = 1;//除广播指令所有指令返回应答
|
||||
u8Status = 0;
|
||||
}
|
||||
|
||||
SCS::SCS(u8 End)
|
||||
{
|
||||
Level = 1;
|
||||
this->End = End;
|
||||
u8Status = 0;
|
||||
}
|
||||
|
||||
SCS::SCS(u8 End, u8 Level)
|
||||
{
|
||||
this->Level = Level;
|
||||
this->End = End;
|
||||
u8Status = 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);
|
||||
}
|
||||
}
|
||||
|
||||
//2个8位数组合为1个16位数
|
||||
//DataL为低位，DataH为高位
|
||||
u16 SCS::SCS2Host(u8 DataL, u8 DataH)
|
||||
{
|
||||
u16 Data;
|
||||
if(End){
|
||||
Data = DataL;
|
||||
Data<<=8;
|
||||
Data |= DataH;
|
||||
}else{
|
||||
Data = DataH;
|
||||
Data<<=8;
|
||||
Data |= DataL;
|
||||
}
|
||||
return Data;
|
||||
}
|
||||
|
||||
void SCS::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);
|
||||
}
|
||||
|
||||
//普通写指令
|
||||
//舵机ID，MemAddr内存表地址，写入数据，写入长度
|
||||
int SCS::genWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen)
|
||||
{
|
||||
rFlushSCS();
|
||||
writeBuf(ID, MemAddr, nDat, nLen, INST_WRITE);
|
||||
wFlushSCS();
|
||||
return Ack(ID);
|
||||
}
|
||||
|
||||
//异步写指令
|
||||
//舵机ID，MemAddr内存表地址，写入数据，写入长度
|
||||
int SCS::regWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen)
|
||||
{
|
||||
rFlushSCS();
|
||||
writeBuf(ID, MemAddr, nDat, nLen, INST_REG_WRITE);
|
||||
wFlushSCS();
|
||||
return Ack(ID);
|
||||
}
|
||||
|
||||
//异步写执行指令
|
||||
//舵机ID
|
||||
int SCS::RegWriteAction(u8 ID)
|
||||
{
|
||||
rFlushSCS();
|
||||
writeBuf(ID, 0, NULL, 0, INST_REG_ACTION);
|
||||
wFlushSCS();
|
||||
return Ack(ID);
|
||||
}
|
||||
|
||||
//同步写指令
|
||||
//舵机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();
|
||||
}
|
||||
|
||||
int SCS::writeByte(u8 ID, u8 MemAddr, u8 bDat)
|
||||
{
|
||||
rFlushSCS();
|
||||
writeBuf(ID, MemAddr, &bDat, 1, INST_WRITE);
|
||||
wFlushSCS();
|
||||
return Ack(ID);
|
||||
}
|
||||
|
||||
int SCS::writeWord(u8 ID, u8 MemAddr, u16 wDat)
|
||||
{
|
||||
u8 bBuf[2];
|
||||
Host2SCS(bBuf+0, bBuf+1, wDat);
|
||||
rFlushSCS();
|
||||
writeBuf(ID, MemAddr, bBuf, 2, INST_WRITE);
|
||||
wFlushSCS();
|
||||
return Ack(ID);
|
||||
}
|
||||
|
||||
//读指令
|
||||
//舵机ID，MemAddr内存表地址，返回数据nData，数据长度nLen
|
||||
int SCS::Read(u8 ID, u8 MemAddr, u8 *nData, u8 nLen)
|
||||
{
|
||||
rFlushSCS();
|
||||
writeBuf(ID, MemAddr, &nLen, 1, INST_READ);
|
||||
wFlushSCS();
|
||||
u8Error = 0;
|
||||
if(!checkHead()){
|
||||
u8Error = ERR_NO_REPLY;
|
||||
return 0;
|
||||
}
|
||||
u8 bBuf[4];
|
||||
u8Status = 0;
|
||||
if(readSCS(bBuf, 3)!=3){
|
||||
u8Error = ERR_NO_REPLY;
|
||||
return 0;
|
||||
}
|
||||
if(bBuf[0]!=ID && ID!=0xfe){
|
||||
u8Error = ERR_SLAVE_ID;
|
||||
return 0;
|
||||
}
|
||||
if(bBuf[1]!=(nLen+2)){
|
||||
u8Error = ERR_BUFF_LEN;
|
||||
return 0;
|
||||
}
|
||||
int Size = readSCS(nData, nLen);
|
||||
if(Size!=nLen){
|
||||
u8Error = ERR_NO_REPLY;
|
||||
return 0;
|
||||
}
|
||||
if(readSCS(bBuf+3, 1)!=1){
|
||||
u8Error = ERR_NO_REPLY;
|
||||
return 0;
|
||||
}
|
||||
u8 calSum = bBuf[0]+bBuf[1]+bBuf[2];
|
||||
u8 i;
|
||||
for(i=0; i<Size; i++){
|
||||
calSum += nData[i];
|
||||
}
|
||||
calSum = ~calSum;
|
||||
if(calSum!=bBuf[3]){
|
||||
u8Error = ERR_CRC_CMP;
|
||||
return 0;
|
||||
}
|
||||
u8Status = bBuf[2];
|
||||
return Size;
|
||||
}
|
||||
|
||||
//读1字节，超时返回-1
|
||||
int SCS::readByte(u8 ID, u8 MemAddr)
|
||||
{
|
||||
u8 bDat;
|
||||
int Size = Read(ID, MemAddr, &bDat, 1);
|
||||
if(Size!=1){
|
||||
return -1;
|
||||
}else{
|
||||
return bDat;
|
||||
}
|
||||
}
|
||||
|
||||
//读2字节，超时返回-1
|
||||
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]);
|
||||
return wDat;
|
||||
}
|
||||
|
||||
//Ping指令，返回舵机ID，超时返回-1
|
||||
int SCS::Ping(u8 ID)
|
||||
{
|
||||
rFlushSCS();
|
||||
writeBuf(ID, 0, NULL, 0, INST_PING);
|
||||
wFlushSCS();
|
||||
u8Status = 0;
|
||||
if(!checkHead()){
|
||||
u8Error = ERR_NO_REPLY;
|
||||
return -1;
|
||||
}
|
||||
u8 bBuf[4];
|
||||
u8Error = 0;
|
||||
if(readSCS(bBuf, 4)!=4){
|
||||
u8Error = ERR_NO_REPLY;
|
||||
return -1;
|
||||
}
|
||||
if(bBuf[0]!=ID && ID!=0xfe){
|
||||
u8Error = ERR_SLAVE_ID;
|
||||
return -1;
|
||||
}
|
||||
if(bBuf[1]!=2){
|
||||
u8Error = ERR_BUFF_LEN;
|
||||
return -1;
|
||||
}
|
||||
u8 calSum = ~(bBuf[0]+bBuf[1]+bBuf[2]);
|
||||
if(calSum!=bBuf[3]){
|
||||
u8Error = ERR_CRC_CMP;
|
||||
return -1;
|
||||
}
|
||||
u8Status = bBuf[2];
|
||||
return bBuf[0];
|
||||
}
|
||||
|
||||
int SCS::checkHead()
|
||||
{
|
||||
u8 bDat;
|
||||
u8 bBuf[] = {0, 0};
|
||||
u8 Cnt = 0;
|
||||
while(1){
|
||||
if(!readSCS(&bDat, 1)){
|
||||
return 0;
|
||||
}
|
||||
bBuf[1] = bBuf[0];
|
||||
bBuf[0] = bDat;
|
||||
if(bBuf[0]==0xff && bBuf[1]==0xff){
|
||||
break;
|
||||
}
|
||||
Cnt++;
|
||||
if(Cnt>10){
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
int SCS::Ack(u8 ID)
|
||||
{
|
||||
u8Error = 0;
|
||||
if(ID!=0xfe && Level){
|
||||
if(!checkHead()){
|
||||
u8Error = ERR_NO_REPLY;
|
||||
return 0;
|
||||
}
|
||||
u8Status = 0;
|
||||
u8 bBuf[4];
|
||||
if(readSCS(bBuf, 4)!=4){
|
||||
u8Error = ERR_NO_REPLY;
|
||||
return 0;
|
||||
}
|
||||
if(bBuf[0]!=ID){
|
||||
u8Error = ERR_SLAVE_ID;
|
||||
return 0;
|
||||
}
|
||||
if(bBuf[1]!=2){
|
||||
u8Error = ERR_BUFF_LEN;
|
||||
return 0;
|
||||
}
|
||||
u8 calSum = ~(bBuf[0]+bBuf[1]+bBuf[2]);
|
||||
if(calSum!=bBuf[3]){
|
||||
u8Error = ERR_CRC_CMP;
|
||||
return 0;
|
||||
}
|
||||
u8Status = bBuf[2];
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
int SCS::syncReadPacketTx(u8 ID[], u8 IDN, u8 MemAddr, u8 nLen)
|
||||
{
|
||||
rFlushSCS();
|
||||
syncReadRxPacketLen = nLen;
|
||||
u8 checkSum = (4+0xfe) + IDN + MemAddr + nLen + INST_SYNC_READ;
|
||||
u8 i;
|
||||
writeSCS(0xff);
|
||||
writeSCS(0xff);
|
||||
writeSCS(0xfe);
|
||||
writeSCS(IDN+4);
|
||||
writeSCS(INST_SYNC_READ);
|
||||
writeSCS(MemAddr);
|
||||
writeSCS(nLen);
|
||||
for(i=0; i<IDN; i++){
|
||||
writeSCS(ID[i]);
|
||||
checkSum += ID[i];
|
||||
}
|
||||
checkSum = ~checkSum;
|
||||
writeSCS(checkSum);
|
||||
wFlushSCS();
|
||||
|
||||
syncReadRxBuffLen = readSCS(syncReadRxBuff, syncReadRxBuffMax, syncTimeOut);
|
||||
return syncReadRxBuffLen;
|
||||
}
|
||||
|
||||
void SCS::syncReadBegin(u8 IDN, u8 rxLen, u32 TimeOut)
|
||||
{
|
||||
syncReadRxBuffMax = IDN*(rxLen+6);
|
||||
syncReadRxBuff = new u8[syncReadRxBuffMax];
|
||||
syncTimeOut = TimeOut;
|
||||
}
|
||||
|
||||
void SCS::syncReadEnd()
|
||||
{
|
||||
if(syncReadRxBuff){
|
||||
delete syncReadRxBuff;
|
||||
syncReadRxBuff = NULL;
|
||||
}
|
||||
}
|
||||
|
||||
int SCS::syncReadPacketRx(u8 ID, u8 *nDat)
|
||||
{
|
||||
u16 syncReadRxBuffIndex = 0;
|
||||
syncReadRxPacket = nDat;
|
||||
syncReadRxPacketIndex = 0;
|
||||
u8Error = 0;
|
||||
while((syncReadRxBuffIndex+6+syncReadRxPacketLen)<=syncReadRxBuffLen){
|
||||
u8 bBuf[] = {0, 0, 0};
|
||||
u8 calSum = 0;
|
||||
while(syncReadRxBuffIndex<syncReadRxBuffLen){
|
||||
bBuf[0] = bBuf[1];
|
||||
bBuf[1] = bBuf[2];
|
||||
bBuf[2] = syncReadRxBuff[syncReadRxBuffIndex++];
|
||||
if(bBuf[0]==0xff && bBuf[1]==0xff && bBuf[2]!=0xff){
|
||||
break;
|
||||
}
|
||||
}
|
||||
if(bBuf[2]!=ID){
|
||||
continue;
|
||||
}
|
||||
if(syncReadRxBuff[syncReadRxBuffIndex++]!=(syncReadRxPacketLen+2)){
|
||||
continue;
|
||||
}
|
||||
u8Status = syncReadRxBuff[syncReadRxBuffIndex++];
|
||||
calSum = ID + (syncReadRxPacketLen+2) + u8Status;
|
||||
for(u8 i=0; i<syncReadRxPacketLen; i++){
|
||||
syncReadRxPacket[i] = syncReadRxBuff[syncReadRxBuffIndex++];
|
||||
calSum += syncReadRxPacket[i];
|
||||
}
|
||||
calSum = ~calSum;
|
||||
if(calSum!=syncReadRxBuff[syncReadRxBuffIndex++]){
|
||||
u8Error = ERR_CRC_CMP;
|
||||
return 0;
|
||||
}
|
||||
return syncReadRxPacketLen;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int SCS::syncReadRxPacketToByte()
|
||||
{
|
||||
if(syncReadRxPacketIndex>=syncReadRxPacketLen){
|
||||
u8Error = ERR_BUFF_LEN;
|
||||
return -1;
|
||||
}
|
||||
return syncReadRxPacket[syncReadRxPacketIndex++];
|
||||
}
|
||||
|
||||
int SCS::syncReadRxPacketToWrod(u8 negBit)
|
||||
{
|
||||
if((syncReadRxPacketIndex+1)>=syncReadRxPacketLen){
|
||||
u8Error = ERR_BUFF_LEN;
|
||||
return -1;
|
||||
}
|
||||
int Word = SCS2Host(syncReadRxPacket[syncReadRxPacketIndex], syncReadRxPacket[syncReadRxPacketIndex+1]);
|
||||
syncReadRxPacketIndex += 2;
|
||||
if(negBit){
|
||||
if(Word&(1<<negBit)){
|
||||
Word = -(Word & ~(1<<negBit));
|
||||
}
|
||||
}
|
||||
return Word;
|
||||
}
|
||||
|
||||
int SCS::Reset(u8 ID)
|
||||
{
|
||||
rFlushSCS();
|
||||
writeBuf(ID, 0, NULL, 0, INST_RESET);
|
||||
wFlushSCS();
|
||||
return Ack(ID);
|
||||
}
|
||||
|
||||
int SCS::Recal(u8 ID)
|
||||
{
|
||||
rFlushSCS();
|
||||
writeBuf(ID, 0, NULL, 0, INST_CAL);
|
||||
wFlushSCS();
|
||||
return Ack(ID);
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
/*
|
||||
* SCS.h
|
||||
* 飞特串行舵机通信层协议程序
|
||||
* 日期: 2024.11.21
|
||||
* 作者: txl
|
||||
*/
|
||||
|
||||
#ifndef _SCS_H
|
||||
#define _SCS_H
|
||||
|
||||
#include "INST.h"
|
||||
|
||||
class SCS{
|
||||
public:
|
||||
SCS();
|
||||
SCS(u8 End);
|
||||
SCS(u8 End, u8 Level);
|
||||
int genWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen);//普通写指令
|
||||
int regWrite(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen);//异步写指令
|
||||
int RegWriteAction(u8 ID = 0xfe);//异步写执行指令
|
||||
void syncWrite(u8 ID[], u8 IDN, u8 MemAddr, u8 *nDat, u8 nLen);//同步写指令
|
||||
int writeByte(u8 ID, u8 MemAddr, u8 bDat);//写1个字节
|
||||
int writeWord(u8 ID, u8 MemAddr, u16 wDat);//写2个字节
|
||||
int Read(u8 ID, u8 MemAddr, u8 *nData, u8 nLen);//读指令
|
||||
int readByte(u8 ID, u8 MemAddr);//读1个字节
|
||||
int readWord(u8 ID, u8 MemAddr);//读2个字节
|
||||
int Ping(u8 ID);//Ping指令
|
||||
int syncReadPacketTx(u8 ID[], u8 IDN, u8 MemAddr, u8 nLen);//同步读指令包发送
|
||||
int syncReadPacketRx(u8 ID, u8 *nDat);//同步读返回包解码，成功返回内存字节数，失败返回0
|
||||
int syncReadRxPacketToByte();//解码一个字节
|
||||
int syncReadRxPacketToWrod(u8 negBit=0);//解码两个字节，negBit为方向为，negBit=0表示无方向
|
||||
void syncReadBegin(u8 IDN, u8 rxLen, u32 TimeOut);//同步读开始
|
||||
void syncReadEnd();//同步读结束
|
||||
int Reset(u8 ID);//重置舵机状态
|
||||
int Recal(u8 ID);//重置舵机中位
|
||||
u8 getState() { return u8Status; }
|
||||
u8 getLastError() { return u8Error; }
|
||||
public:
|
||||
u8 Level;//舵机返回等级
|
||||
u8 End;//处理器大小端结构
|
||||
u8 u8Status;//舵机状态
|
||||
u8 u8Error;//通信状态
|
||||
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 readSCS(unsigned char *nDat, int nLen) = 0;
|
||||
virtual int readSCS(unsigned char *nDat, int nLen, unsigned long TimeOut) = 0;
|
||||
virtual int writeSCS(unsigned char bDat) = 0;
|
||||
virtual void rFlushSCS() = 0;
|
||||
virtual void wFlushSCS() = 0;
|
||||
protected:
|
||||
void writeBuf(u8 ID, u8 MemAddr, u8 *nDat, u8 nLen, u8 Fun);
|
||||
void Host2SCS(u8 *DataL, u8* DataH, u16 Data);//1个16位数拆分为2个8位数
|
||||
u16 SCS2Host(u8 DataL, u8 DataH);//2个8位数组合为1个16位数
|
||||
int Ack(u8 ID);//返回应答
|
||||
int checkHead();//帧头检测
|
||||
};
|
||||
#endif
|
||||
@@ -0,0 +1,243 @@
|
||||
/*
|
||||
* SCSCL.cpp
|
||||
* 飞特SCSCL系列串行舵机应用层程序
|
||||
* 日期: 2024.4.2
|
||||
* 作者:
|
||||
*/
|
||||
|
||||
#include "SCSCL.h"
|
||||
|
||||
SCSCL::SCSCL()
|
||||
{
|
||||
End = 1;
|
||||
}
|
||||
|
||||
SCSCL::SCSCL(u8 End):SCSerial(End)
|
||||
{
|
||||
}
|
||||
|
||||
SCSCL::SCSCL(u8 End, u8 Level):SCSerial(End, Level)
|
||||
{
|
||||
}
|
||||
|
||||
int SCSCL::WritePos(u8 ID, u16 Position, u16 Time, u16 Speed)
|
||||
{
|
||||
u8 bBuf[6];
|
||||
Host2SCS(bBuf+0, bBuf+1, Position);
|
||||
Host2SCS(bBuf+2, bBuf+3, Time);
|
||||
Host2SCS(bBuf+4, bBuf+5, Speed);
|
||||
|
||||
return genWrite(ID, SCSCL_GOAL_POSITION_L, bBuf, 6);
|
||||
}
|
||||
|
||||
int SCSCL::RegWritePos(u8 ID, u16 Position, u16 Time, u16 Speed)
|
||||
{
|
||||
u8 bBuf[6];
|
||||
Host2SCS(bBuf+0, bBuf+1, Position);
|
||||
Host2SCS(bBuf+2, bBuf+3, Time);
|
||||
Host2SCS(bBuf+4, bBuf+5, Speed);
|
||||
|
||||
return regWrite(ID, SCSCL_GOAL_POSITION_L, bBuf, 6);
|
||||
}
|
||||
|
||||
void SCSCL::SyncWritePos(u8 ID[], u8 IDN, u16 Position[], u16 Time[], u16 Speed[])
|
||||
{
|
||||
u8 offbuf[6*IDN];
|
||||
for(u8 i = 0; i<IDN; i++){
|
||||
u16 T, V;
|
||||
if(Time){
|
||||
T = Time[i];
|
||||
}else{
|
||||
T = 0;
|
||||
}
|
||||
if(Speed){
|
||||
V = Speed[i];
|
||||
}else{
|
||||
V = 0;
|
||||
}
|
||||
Host2SCS(offbuf+i*6+0, offbuf+i*6+1, Position[i]);
|
||||
Host2SCS(offbuf+i*6+2, offbuf+i*6+3, T);
|
||||
Host2SCS(offbuf+i*6+4, offbuf+i*6+5, V);
|
||||
}
|
||||
syncWrite(ID, IDN, SCSCL_GOAL_POSITION_L, offbuf, 6);
|
||||
}
|
||||
|
||||
int SCSCL::EnableTorque(u8 ID, u8 Enable)
|
||||
{
|
||||
return writeByte(ID, SCSCL_TORQUE_ENABLE, Enable);
|
||||
}
|
||||
|
||||
int SCSCL::unLockEprom(u8 ID)
|
||||
{
|
||||
return writeByte(ID, SCSCL_LOCK, 0);
|
||||
}
|
||||
|
||||
int SCSCL::LockEprom(u8 ID)
|
||||
{
|
||||
return writeByte(ID, SCSCL_LOCK, 1);
|
||||
}
|
||||
|
||||
int SCSCL::FeedBack(int ID)
|
||||
{
|
||||
int nLen = Read(ID, SCSCL_PRESENT_POSITION_L, Mem, sizeof(Mem));
|
||||
if(nLen!=sizeof(Mem)){
|
||||
return -1;
|
||||
}
|
||||
return nLen;
|
||||
}
|
||||
|
||||
int SCSCL::ReadPos(int ID)
|
||||
{
|
||||
int Pos = -1;
|
||||
if(ID==-1){
|
||||
Pos = Mem[SCSCL_PRESENT_POSITION_L-SCSCL_PRESENT_POSITION_L];
|
||||
Pos <<= 8;
|
||||
Pos |= Mem[SCSCL_PRESENT_POSITION_H-SCSCL_PRESENT_POSITION_L];
|
||||
}else{
|
||||
Pos = readWord(ID, SCSCL_PRESENT_POSITION_L);
|
||||
}
|
||||
return Pos;
|
||||
}
|
||||
|
||||
int SCSCL::ReadSpeed(int ID)
|
||||
{
|
||||
int Speed = -1;
|
||||
if(ID==-1){
|
||||
Speed = Mem[SCSCL_PRESENT_SPEED_L-SCSCL_PRESENT_POSITION_L];
|
||||
Speed <<= 8;
|
||||
Speed |= Mem[SCSCL_PRESENT_SPEED_H-SCSCL_PRESENT_POSITION_L];
|
||||
}else{
|
||||
Speed = readWord(ID, SCSCL_PRESENT_SPEED_L);
|
||||
}
|
||||
if(Speed&(1<<15)){
|
||||
Speed = -(Speed&~(1<<15));
|
||||
}
|
||||
return Speed;
|
||||
}
|
||||
|
||||
int SCSCL::ReadLoad(int ID)
|
||||
{
|
||||
int Load = -1;
|
||||
if(ID==-1){
|
||||
Load = Mem[SCSCL_PRESENT_LOAD_L-SCSCL_PRESENT_POSITION_L];
|
||||
Load <<= 8;
|
||||
Load |= Mem[SCSCL_PRESENT_LOAD_H-SCSCL_PRESENT_POSITION_L];
|
||||
}else{
|
||||
Load = readWord(ID, SCSCL_PRESENT_LOAD_L);
|
||||
}
|
||||
if(Load&(1<<10)){
|
||||
Load = -(Load&~(1<<10));
|
||||
}
|
||||
return Load;
|
||||
}
|
||||
|
||||
int SCSCL::ReadVoltage(int ID)
|
||||
{
|
||||
int Voltage = -1;
|
||||
if(ID==-1){
|
||||
Voltage = Mem[SCSCL_PRESENT_VOLTAGE-SCSCL_PRESENT_POSITION_L];
|
||||
}else{
|
||||
Voltage = readByte(ID, SCSCL_PRESENT_VOLTAGE);
|
||||
}
|
||||
return Voltage;
|
||||
}
|
||||
|
||||
int SCSCL::ReadTemper(int ID)
|
||||
{
|
||||
int Temper = -1;
|
||||
if(ID==-1){
|
||||
Temper = Mem[SCSCL_PRESENT_TEMPERATURE-SCSCL_PRESENT_POSITION_L];
|
||||
}else{
|
||||
Temper = readByte(ID, SCSCL_PRESENT_TEMPERATURE);
|
||||
}
|
||||
return Temper;
|
||||
}
|
||||
|
||||
int SCSCL::ReadMove(int ID)
|
||||
{
|
||||
int Move = -1;
|
||||
if(ID==-1){
|
||||
Move = Mem[SCSCL_MOVING-SCSCL_PRESENT_POSITION_L];
|
||||
}else{
|
||||
Move = readByte(ID, SCSCL_MOVING);
|
||||
}
|
||||
return Move;
|
||||
}
|
||||
|
||||
int SCSCL::ReadCurrent(int ID)
|
||||
{
|
||||
int Current = -1;
|
||||
if(ID==-1){
|
||||
Current = Mem[SCSCL_PRESENT_CURRENT_L-SCSCL_PRESENT_POSITION_L];
|
||||
Current <<= 8;
|
||||
Current |= Mem[SCSCL_PRESENT_CURRENT_H-SCSCL_PRESENT_POSITION_L];
|
||||
}else{
|
||||
Current = readWord(ID, SCSCL_PRESENT_CURRENT_L);
|
||||
}
|
||||
if(Current&(1<<15)){
|
||||
Current = -(Current&~(1<<15));
|
||||
}
|
||||
return Current;
|
||||
}
|
||||
|
||||
int SCSCL::PWMMode(u8 ID)
|
||||
{
|
||||
u8 bBuf[4];
|
||||
bBuf[0] = 0;
|
||||
bBuf[1] = 0;
|
||||
bBuf[2] = 0;
|
||||
bBuf[3] = 0;
|
||||
return genWrite(ID, SCSCL_MIN_ANGLE_LIMIT_L, bBuf, 4);
|
||||
}
|
||||
|
||||
int SCSCL::WritePWM(u8 ID, s16 pwmOut)
|
||||
{
|
||||
if(pwmOut<0){
|
||||
pwmOut = -pwmOut;
|
||||
pwmOut |= (1<<10);
|
||||
}
|
||||
u8 bBuf[2];
|
||||
Host2SCS(bBuf+0, bBuf+1, pwmOut);
|
||||
|
||||
return genWrite(ID, SCSCL_GOAL_TIME_L, bBuf, 2);
|
||||
}
|
||||
|
||||
int SCSCL::SwitchMode(int ID, u8 mode)
|
||||
{
|
||||
if(ID < 0 || ID > 0xff){
|
||||
return -1;
|
||||
}
|
||||
if(mode > 1){
|
||||
return -2;
|
||||
}
|
||||
|
||||
const u8 servo_id = static_cast<u8>(ID);
|
||||
|
||||
if(mode == 1){
|
||||
min_angle[servo_id] = readWord(servo_id, SCSCL_MIN_ANGLE_LIMIT_L);
|
||||
max_angle[servo_id] = readWord(servo_id, SCSCL_MAX_ANGLE_LIMIT_L);
|
||||
if(min_angle[servo_id] == -1 || max_angle[servo_id] == -1){
|
||||
return -3;
|
||||
}
|
||||
|
||||
angle_limit_cached[servo_id] = true;
|
||||
return PWMMode(servo_id);
|
||||
}
|
||||
|
||||
if(!angle_limit_cached[servo_id]){
|
||||
return -4;
|
||||
}
|
||||
if(writeWord(servo_id, SCSCL_MIN_ANGLE_LIMIT_L, (u16)min_angle[servo_id]) != 1){
|
||||
return -5;
|
||||
}
|
||||
if(writeWord(servo_id, SCSCL_MAX_ANGLE_LIMIT_L, (u16)max_angle[servo_id]) != 1){
|
||||
return -6;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int SCSCL::ReadToqueEnable(int ID)
|
||||
{
|
||||
return readWord(ID, SCSCL_TORQUE_ENABLE);
|
||||
}
|
||||
@@ -0,0 +1,82 @@
|
||||
/*
|
||||
* SCSCL.h
|
||||
* 飞特SCSCL系列串行舵机应用层程序
|
||||
* 日期: 2024.4.2
|
||||
* 作者:
|
||||
*/
|
||||
|
||||
#ifndef _SCSCL_H
|
||||
#define _SCSCL_H
|
||||
|
||||
//内存表定义
|
||||
//-------EPROM(只读)--------
|
||||
#define SCSCL_VERSION_L 3
|
||||
#define SCSCL_VERSION_H 4
|
||||
|
||||
//-------EPROM(读写)--------
|
||||
#define SCSCL_ID 5
|
||||
#define SCSCL_BAUD_RATE 6
|
||||
#define SCSCL_MIN_ANGLE_LIMIT_L 9
|
||||
#define SCSCL_MIN_ANGLE_LIMIT_H 10
|
||||
#define SCSCL_MAX_ANGLE_LIMIT_L 11
|
||||
#define SCSCL_MAX_ANGLE_LIMIT_H 12
|
||||
#define SCSCL_CW_DEAD 26
|
||||
#define SCSCL_CCW_DEAD 27
|
||||
|
||||
//-------SRAM(读写)--------
|
||||
#define SCSCL_TORQUE_ENABLE 40
|
||||
#define SCSCL_GOAL_POSITION_L 42
|
||||
#define SCSCL_GOAL_POSITION_H 43
|
||||
#define SCSCL_GOAL_TIME_L 44
|
||||
#define SCSCL_GOAL_TIME_H 45
|
||||
#define SCSCL_GOAL_SPEED_L 46
|
||||
#define SCSCL_GOAL_SPEED_H 47
|
||||
#define SCSCL_LOCK 48
|
||||
|
||||
//-------SRAM(只读)--------
|
||||
#define SCSCL_PRESENT_POSITION_L 56
|
||||
#define SCSCL_PRESENT_POSITION_H 57
|
||||
#define SCSCL_PRESENT_SPEED_L 58
|
||||
#define SCSCL_PRESENT_SPEED_H 59
|
||||
#define SCSCL_PRESENT_LOAD_L 60
|
||||
#define SCSCL_PRESENT_LOAD_H 61
|
||||
#define SCSCL_PRESENT_VOLTAGE 62
|
||||
#define SCSCL_PRESENT_TEMPERATURE 63
|
||||
#define SCSCL_MOVING 66
|
||||
#define SCSCL_PRESENT_CURRENT_L 69
|
||||
#define SCSCL_PRESENT_CURRENT_H 70
|
||||
|
||||
#include "SCSerial.h"
|
||||
|
||||
class SCSCL : public SCSerial
|
||||
{
|
||||
public:
|
||||
SCSCL();
|
||||
SCSCL(u8 End);
|
||||
SCSCL(u8 End, u8 Level);
|
||||
int WritePos(u8 ID, u16 Position, u16 Time, u16 Speed = 0);//普通写单个舵机位置指令
|
||||
int RegWritePos(u8 ID, u16 Position, u16 Time, u16 Speed = 0);//异步写单个舵机位置指令(RegWriteAction生效)
|
||||
void SyncWritePos(u8 ID[], u8 IDN, u16 Position[], u16 Time[], u16 Speed[]);//同步写多个舵机位置指令
|
||||
int PWMMode(u8 ID);//PWM模式
|
||||
int WritePWM(u8 ID, s16 pwmOut);//PWM输出模式指令
|
||||
int SwitchMode(int ID, u8 mode);//切换位置模式和PWM模式
|
||||
int EnableTorque(u8 ID, u8 Enable);//扭矩控制指令
|
||||
int unLockEprom(u8 ID);//eprom解锁
|
||||
int LockEprom(u8 ID);//eprom加锁
|
||||
int FeedBack(int ID);//反馈舵机信息
|
||||
int ReadPos(int ID);//读位置
|
||||
int ReadSpeed(int ID);//读速度
|
||||
int ReadLoad(int ID);//读输出至电机的电压百分比(0~1000)
|
||||
int ReadVoltage(int ID);//读电压
|
||||
int ReadTemper(int ID);//读温度
|
||||
int ReadMove(int ID);//读移动状态
|
||||
int ReadCurrent(int ID);//读电流
|
||||
int ReadToqueEnable(int ID);//读扭矩使能状态
|
||||
private:
|
||||
u8 Mem[SCSCL_PRESENT_CURRENT_H-SCSCL_PRESENT_POSITION_L+1];
|
||||
int min_angle[256] = {0};
|
||||
int max_angle[256] = {0};
|
||||
bool angle_limit_cached[256] = {false};
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,191 @@
|
||||
/*
|
||||
* SCSerial.cpp
|
||||
* 飞特串行舵机硬件接口层程序
|
||||
* 日期: 2024.4.2
|
||||
* 作者:
|
||||
*/
|
||||
|
||||
#include "SCSerial.h"
|
||||
|
||||
#include "esp_log.h"
|
||||
#include "esp_timer.h"
|
||||
|
||||
static const char *TAG = "FT_SCSerial";
|
||||
|
||||
SCSerial::SCSerial()
|
||||
{
|
||||
IOTimeOut = 10;
|
||||
uart_num = UART_NUM_MAX;
|
||||
}
|
||||
|
||||
SCSerial::SCSerial(u8 End):SCS(End)
|
||||
{
|
||||
IOTimeOut = 10;
|
||||
uart_num = UART_NUM_MAX;
|
||||
}
|
||||
|
||||
SCSerial::SCSerial(u8 End, u8 Level):SCS(End, Level)
|
||||
{
|
||||
IOTimeOut = 10;
|
||||
uart_num = UART_NUM_MAX;
|
||||
}
|
||||
|
||||
bool SCSerial::begin(uart_port_t uart_num, int baud_rate, int tx_pin, int rx_pin, int buf_size)
|
||||
{
|
||||
this->uart_num = uart_num;
|
||||
|
||||
uart_config_t uart_config = {
|
||||
.baud_rate = baud_rate,
|
||||
.data_bits = UART_DATA_8_BITS,
|
||||
.parity = UART_PARITY_DISABLE,
|
||||
.stop_bits = UART_STOP_BITS_1,
|
||||
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
|
||||
.rx_flow_ctrl_thresh = 0,
|
||||
.source_clk = UART_SCLK_DEFAULT,
|
||||
};
|
||||
|
||||
esp_err_t ret = uart_driver_install(uart_num, buf_size, buf_size, 0, NULL, 0);
|
||||
if (ret != ESP_OK) {
|
||||
ESP_LOGE(TAG, "Failed to install UART driver: %s", esp_err_to_name(ret));
|
||||
return false;
|
||||
}
|
||||
|
||||
ret = uart_param_config(uart_num, &uart_config);
|
||||
if (ret != ESP_OK) {
|
||||
ESP_LOGE(TAG, "Failed to configure UART: %s", esp_err_to_name(ret));
|
||||
uart_driver_delete(uart_num);
|
||||
this->uart_num = UART_NUM_MAX;
|
||||
return false;
|
||||
}
|
||||
|
||||
ret = uart_set_pin(uart_num, tx_pin, rx_pin, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
|
||||
if (ret != ESP_OK) {
|
||||
ESP_LOGE(TAG, "Failed to set UART pins: %s", esp_err_to_name(ret));
|
||||
uart_driver_delete(uart_num);
|
||||
this->uart_num = UART_NUM_MAX;
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void SCSerial::end()
|
||||
{
|
||||
if (uart_num < UART_NUM_MAX) {
|
||||
uart_driver_delete(uart_num);
|
||||
uart_num = UART_NUM_MAX;
|
||||
}
|
||||
}
|
||||
|
||||
int SCSerial::readSCS(unsigned char *nDat, int nLen, unsigned long TimeOut)
|
||||
{
|
||||
if (uart_num >= UART_NUM_MAX) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
int size = 0;
|
||||
int64_t t_begin = esp_timer_get_time() / 1000;
|
||||
|
||||
while (1) {
|
||||
size_t available = 0;
|
||||
uart_get_buffered_data_len(uart_num, &available);
|
||||
|
||||
if (available > 0) {
|
||||
unsigned char byte;
|
||||
int len = uart_read_bytes(uart_num, &byte, 1, 0);
|
||||
if (len > 0) {
|
||||
if (nDat) {
|
||||
nDat[size] = byte;
|
||||
}
|
||||
size++;
|
||||
}
|
||||
}
|
||||
|
||||
if (size >= nLen) {
|
||||
break;
|
||||
}
|
||||
|
||||
if ((esp_timer_get_time() / 1000) - t_begin > static_cast<int64_t>(TimeOut)) {
|
||||
break;
|
||||
}
|
||||
|
||||
vTaskDelay(pdMS_TO_TICKS(1));
|
||||
}
|
||||
|
||||
return size;
|
||||
}
|
||||
|
||||
int SCSerial::readSCS(unsigned char *nDat, int nLen)
|
||||
{
|
||||
if (uart_num >= UART_NUM_MAX) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
int size = 0;
|
||||
int64_t t_begin = esp_timer_get_time() / 1000;
|
||||
|
||||
while (1) {
|
||||
size_t available = 0;
|
||||
uart_get_buffered_data_len(uart_num, &available);
|
||||
|
||||
if (available > 0) {
|
||||
unsigned char byte;
|
||||
int len = uart_read_bytes(uart_num, &byte, 1, 0);
|
||||
if (len > 0) {
|
||||
if (nDat) {
|
||||
nDat[size] = byte;
|
||||
}
|
||||
size++;
|
||||
t_begin = esp_timer_get_time() / 1000;
|
||||
}
|
||||
}
|
||||
|
||||
if (size >= nLen) {
|
||||
break;
|
||||
}
|
||||
|
||||
if ((esp_timer_get_time() / 1000) - t_begin > static_cast<int64_t>(IOTimeOut)) {
|
||||
break;
|
||||
}
|
||||
|
||||
vTaskDelay(pdMS_TO_TICKS(1));
|
||||
}
|
||||
|
||||
return size;
|
||||
}
|
||||
|
||||
int SCSerial::writeSCS(unsigned char *nDat, int nLen)
|
||||
{
|
||||
if (uart_num >= UART_NUM_MAX || nDat == NULL) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
return uart_write_bytes(uart_num, reinterpret_cast<const char *>(nDat), nLen);
|
||||
}
|
||||
|
||||
int SCSerial::writeSCS(unsigned char bDat)
|
||||
{
|
||||
if (uart_num >= UART_NUM_MAX) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
return uart_write_bytes(uart_num, reinterpret_cast<const char *>(&bDat), 1);
|
||||
}
|
||||
|
||||
void SCSerial::rFlushSCS()
|
||||
{
|
||||
if (uart_num >= UART_NUM_MAX) {
|
||||
return;
|
||||
}
|
||||
|
||||
uart_flush_input(uart_num);
|
||||
}
|
||||
|
||||
void SCSerial::wFlushSCS()
|
||||
{
|
||||
if (uart_num >= UART_NUM_MAX) {
|
||||
return;
|
||||
}
|
||||
|
||||
uart_wait_tx_done(uart_num, pdMS_TO_TICKS(100));
|
||||
}
|
||||
@@ -0,0 +1,39 @@
|
||||
/*
|
||||
* SCSerial.h
|
||||
* 飞特串行舵机硬件接口层程序
|
||||
* 日期: 2024.11.22
|
||||
* 作者: txl
|
||||
*/
|
||||
|
||||
#ifndef _SCSERIAL_H
|
||||
#define _SCSERIAL_H
|
||||
|
||||
#include "driver/gpio.h"
|
||||
#include "driver/uart.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
|
||||
#include "SCS.h"
|
||||
|
||||
class SCSerial : public SCS
|
||||
{
|
||||
public:
|
||||
SCSerial();
|
||||
SCSerial(u8 End);
|
||||
SCSerial(u8 End, u8 Level);
|
||||
bool begin(uart_port_t uart_num, int baud_rate, int tx_pin, int rx_pin, int buf_size = 1024);
|
||||
void end();
|
||||
|
||||
protected:
|
||||
int writeSCS(unsigned char *nDat, int nLen);//输出nLen字节
|
||||
int readSCS(unsigned char *nDat, int nLen);//输入nLen字节
|
||||
int readSCS(unsigned char *nDat, int nLen, unsigned long TimeOut);
|
||||
int writeSCS(unsigned char bDat);//输出1字节
|
||||
void rFlushSCS();//
|
||||
void wFlushSCS();//
|
||||
public:
|
||||
unsigned long IOTimeOut;//输入输出超时
|
||||
uart_port_t uart_num;//串口编号
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,15 @@
|
||||
/*
|
||||
* SCServo.h
|
||||
* 飞特串行舵机接口
|
||||
* 日期: 2024.11.24
|
||||
* 作者: txl
|
||||
*/
|
||||
|
||||
#ifndef _SCSERVO_H
|
||||
#define _SCSERVO_H
|
||||
|
||||
#include "SCSCL.h"
|
||||
#include "SMS_STS.h"
|
||||
#include "HLSCL.h"
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,249 @@
|
||||
/*
|
||||
* SMS_STS.cpp
|
||||
* 飞特SMS_STS系列串行舵机应用层程序
|
||||
* 日期: 2024.11.21
|
||||
* 作者: txl
|
||||
*/
|
||||
|
||||
#include "SMS_STS.h"
|
||||
|
||||
SMS_STS::SMS_STS()
|
||||
{
|
||||
End = 0;
|
||||
}
|
||||
|
||||
SMS_STS::SMS_STS(u8 End):SCSerial(End)
|
||||
{
|
||||
}
|
||||
|
||||
SMS_STS::SMS_STS(u8 End, u8 Level):SCSerial(End, Level)
|
||||
{
|
||||
}
|
||||
|
||||
int SMS_STS::WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC)
|
||||
{
|
||||
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, 0);
|
||||
Host2SCS(bBuf+5, bBuf+6, Speed);
|
||||
|
||||
return genWrite(ID, SMS_STS_ACC, bBuf, 7);
|
||||
}
|
||||
|
||||
int SMS_STS::RegWritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC)
|
||||
{
|
||||
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, 0);
|
||||
Host2SCS(bBuf+5, bBuf+6, Speed);
|
||||
|
||||
return regWrite(ID, SMS_STS_ACC, bBuf, 7);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
void SMS_STS::SyncWriteSpe(u8 ID[], u8 IDN, s16 Speed[], u8 ACC[])
|
||||
{
|
||||
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, 0);
|
||||
Host2SCS(offbuf+i*7+5, offbuf+i*7+6, Speed[i]);
|
||||
}
|
||||
syncWrite(ID, IDN, SMS_STS_ACC, offbuf, 7);
|
||||
}
|
||||
|
||||
int SMS_STS::WheelMode(u8 ID)
|
||||
{
|
||||
return writeByte(ID, SMS_STS_MODE, 1);
|
||||
}
|
||||
|
||||
int SMS_STS::WriteSpe(u8 ID, s16 Speed, u8 ACC)
|
||||
{
|
||||
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, 0);
|
||||
Host2SCS(bBuf+5, bBuf+6, Speed);
|
||||
|
||||
return genWrite(ID, SMS_STS_ACC, bBuf, 7);
|
||||
}
|
||||
|
||||
int SMS_STS::EnableTorque(u8 ID, u8 Enable)
|
||||
{
|
||||
return writeByte(ID, SMS_STS_TORQUE_ENABLE, Enable);
|
||||
}
|
||||
|
||||
int SMS_STS::unLockEprom(u8 ID)
|
||||
{
|
||||
return writeByte(ID, SMS_STS_LOCK, 0);
|
||||
}
|
||||
|
||||
int SMS_STS::LockEprom(u8 ID)
|
||||
{
|
||||
return writeByte(ID, SMS_STS_LOCK, 1);
|
||||
}
|
||||
|
||||
int SMS_STS::CalibrationOfs(u8 ID)
|
||||
{
|
||||
return writeByte(ID, SMS_STS_TORQUE_ENABLE, 128);
|
||||
}
|
||||
|
||||
int SMS_STS::FeedBack(int ID)
|
||||
{
|
||||
int nLen = Read(ID, SMS_STS_PRESENT_POSITION_L, Mem, sizeof(Mem));
|
||||
if(nLen!=sizeof(Mem)){
|
||||
return -1;
|
||||
}
|
||||
return nLen;
|
||||
}
|
||||
|
||||
int SMS_STS::ReadPos(int ID)
|
||||
{
|
||||
int Pos = -1;
|
||||
if(ID==-1){
|
||||
Pos = Mem[SMS_STS_PRESENT_POSITION_H-SMS_STS_PRESENT_POSITION_L];
|
||||
Pos <<= 8;
|
||||
Pos |= Mem[SMS_STS_PRESENT_POSITION_L-SMS_STS_PRESENT_POSITION_L];
|
||||
}else{
|
||||
Pos = readWord(ID, SMS_STS_PRESENT_POSITION_L);
|
||||
}
|
||||
if(Pos&(1<<15)){
|
||||
Pos = -(Pos&~(1<<15));
|
||||
}
|
||||
|
||||
return Pos;
|
||||
}
|
||||
|
||||
int SMS_STS::ReadSpeed(int ID)
|
||||
{
|
||||
int Speed = -1;
|
||||
if(ID==-1){
|
||||
Speed = Mem[SMS_STS_PRESENT_SPEED_H-SMS_STS_PRESENT_POSITION_L];
|
||||
Speed <<= 8;
|
||||
Speed |= Mem[SMS_STS_PRESENT_SPEED_L-SMS_STS_PRESENT_POSITION_L];
|
||||
}else{
|
||||
Speed = readWord(ID, SMS_STS_PRESENT_SPEED_L);
|
||||
}
|
||||
if(Speed&(1<<15)){
|
||||
Speed = -(Speed&~(1<<15));
|
||||
}
|
||||
return Speed;
|
||||
}
|
||||
|
||||
int SMS_STS::ReadLoad(int ID)
|
||||
{
|
||||
int Load = -1;
|
||||
if(ID==-1){
|
||||
Load = Mem[SMS_STS_PRESENT_LOAD_H-SMS_STS_PRESENT_POSITION_L];
|
||||
Load <<= 8;
|
||||
Load |= Mem[SMS_STS_PRESENT_LOAD_L-SMS_STS_PRESENT_POSITION_L];
|
||||
}else{
|
||||
Load = readWord(ID, SMS_STS_PRESENT_LOAD_L);
|
||||
}
|
||||
if(Load&(1<<10)){
|
||||
Load = -(Load&~(1<<10));
|
||||
}
|
||||
return Load;
|
||||
}
|
||||
|
||||
int SMS_STS::ReadVoltage(int ID)
|
||||
{
|
||||
int Voltage = -1;
|
||||
if(ID==-1){
|
||||
Voltage = Mem[SMS_STS_PRESENT_VOLTAGE-SMS_STS_PRESENT_POSITION_L];
|
||||
}else{
|
||||
Voltage = readByte(ID, SMS_STS_PRESENT_VOLTAGE);
|
||||
}
|
||||
return Voltage;
|
||||
}
|
||||
|
||||
int SMS_STS::ReadTemper(int ID)
|
||||
{
|
||||
int Temper = -1;
|
||||
if(ID==-1){
|
||||
Temper = Mem[SMS_STS_PRESENT_TEMPERATURE-SMS_STS_PRESENT_POSITION_L];
|
||||
}else{
|
||||
Temper = readByte(ID, SMS_STS_PRESENT_TEMPERATURE);
|
||||
}
|
||||
return Temper;
|
||||
}
|
||||
|
||||
int SMS_STS::ReadMove(int ID)
|
||||
{
|
||||
int Move = -1;
|
||||
if(ID==-1){
|
||||
Move = Mem[SMS_STS_MOVING-SMS_STS_PRESENT_POSITION_L];
|
||||
}else{
|
||||
Move = readByte(ID, SMS_STS_MOVING);
|
||||
}
|
||||
return Move;
|
||||
}
|
||||
|
||||
int SMS_STS::ReadCurrent(int ID)
|
||||
{
|
||||
int Current = -1;
|
||||
if(ID==-1){
|
||||
Current = Mem[SMS_STS_PRESENT_CURRENT_H-SMS_STS_PRESENT_POSITION_L];
|
||||
Current <<= 8;
|
||||
Current |= Mem[SMS_STS_PRESENT_CURRENT_L-SMS_STS_PRESENT_POSITION_L];
|
||||
}else{
|
||||
Current = readWord(ID, SMS_STS_PRESENT_CURRENT_L);
|
||||
}
|
||||
if(Current&(1<<15)){
|
||||
Current = -(Current&~(1<<15));
|
||||
}
|
||||
return Current;
|
||||
}
|
||||
|
||||
int SMS_STS::ServoMode(u8 ID)
|
||||
{
|
||||
return writeByte(ID, SMS_STS_MODE, 0);
|
||||
}
|
||||
@@ -0,0 +1,86 @@
|
||||
/*
|
||||
* SMS_STS.h
|
||||
* 飞特SMS/STS系列串行舵机应用层程序
|
||||
* 日期: 2024.11.21
|
||||
* 作者: txl
|
||||
*/
|
||||
|
||||
#ifndef _SMS_STS_H
|
||||
#define _SMS_STS_H
|
||||
|
||||
//内存表定义
|
||||
//-------EPROM(只读)--------
|
||||
#define SMS_STS_MODEL_L 3
|
||||
#define SMS_STS_MODEL_H 4
|
||||
|
||||
//-------EPROM(读写)--------
|
||||
#define SMS_STS_ID 5
|
||||
#define SMS_STS_BAUD_RATE 6
|
||||
#define SMS_STS_MIN_ANGLE_LIMIT_L 9
|
||||
#define SMS_STS_MIN_ANGLE_LIMIT_H 10
|
||||
#define SMS_STS_MAX_ANGLE_LIMIT_L 11
|
||||
#define SMS_STS_MAX_ANGLE_LIMIT_H 12
|
||||
#define SMS_STS_CW_DEAD 26
|
||||
#define SMS_STS_CCW_DEAD 27
|
||||
#define SMS_STS_OFS_L 31
|
||||
#define SMS_STS_OFS_H 32
|
||||
#define SMS_STS_MODE 33
|
||||
|
||||
//-------SRAM(读写)--------
|
||||
#define SMS_STS_TORQUE_ENABLE 40
|
||||
#define SMS_STS_ACC 41
|
||||
#define SMS_STS_GOAL_POSITION_L 42
|
||||
#define SMS_STS_GOAL_POSITION_H 43
|
||||
#define SMS_STS_GOAL_TIME_L 44
|
||||
#define SMS_STS_GOAL_TIME_H 45
|
||||
#define SMS_STS_GOAL_SPEED_L 46
|
||||
#define SMS_STS_GOAL_SPEED_H 47
|
||||
#define SMS_STS_TORQUE_LIMIT_L 48
|
||||
#define SMS_STS_TORQUE_LIMIT_H 49
|
||||
#define SMS_STS_LOCK 55
|
||||
|
||||
//-------SRAM(只读)--------
|
||||
#define SMS_STS_PRESENT_POSITION_L 56
|
||||
#define SMS_STS_PRESENT_POSITION_H 57
|
||||
#define SMS_STS_PRESENT_SPEED_L 58
|
||||
#define SMS_STS_PRESENT_SPEED_H 59
|
||||
#define SMS_STS_PRESENT_LOAD_L 60
|
||||
#define SMS_STS_PRESENT_LOAD_H 61
|
||||
#define SMS_STS_PRESENT_VOLTAGE 62
|
||||
#define SMS_STS_PRESENT_TEMPERATURE 63
|
||||
#define SMS_STS_MOVING 66
|
||||
#define SMS_STS_PRESENT_CURRENT_L 69
|
||||
#define SMS_STS_PRESENT_CURRENT_H 70
|
||||
|
||||
#include "SCSerial.h"
|
||||
|
||||
class SMS_STS : public SCSerial
|
||||
{
|
||||
public:
|
||||
SMS_STS();
|
||||
SMS_STS(u8 End);
|
||||
SMS_STS(u8 End, u8 Level);
|
||||
int WritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0);//普通写单个舵机位置指令
|
||||
int RegWritePosEx(u8 ID, s16 Position, u16 Speed, u8 ACC = 0);//异步写单个舵机位置指令(RegWriteAction生效)
|
||||
void SyncWritePosEx(u8 ID[], u8 IDN, s16 Position[], u16 Speed[], u8 ACC[]);//同步写多个舵机位置指令
|
||||
void SyncWriteSpe(u8 ID[], u8 IDN, s16 Speed[], u8 ACC[]);//同步写多个舵机速度指令
|
||||
int ServoMode(u8 ID);//Servo模式
|
||||
int WheelMode(u8 ID);//恒速模式
|
||||
int WriteSpe(u8 ID, s16 Speed, u8 ACC = 0);//恒速模式控制指令
|
||||
int EnableTorque(u8 ID, u8 Enable);//扭力控制指令
|
||||
int unLockEprom(u8 ID);//eprom解锁
|
||||
int LockEprom(u8 ID);//eprom加锁
|
||||
int CalibrationOfs(u8 ID);//中位校准
|
||||
int FeedBack(int ID);//反馈舵机信息
|
||||
int ReadPos(int ID);//读位置
|
||||
int ReadSpeed(int ID);//读速度
|
||||
int ReadLoad(int ID);//读输出至电机的电压百分比(0~1000)
|
||||
int ReadVoltage(int ID);//读电压
|
||||
int ReadTemper(int ID);//读温度
|
||||
int ReadMove(int ID);//读移动状态
|
||||
int ReadCurrent(int ID);//读电流
|
||||
private:
|
||||
u8 Mem[SMS_STS_PRESENT_CURRENT_H-SMS_STS_PRESENT_POSITION_L+1];
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -1,12 +0,0 @@
|
||||
idf_component_register(
|
||||
SRCS
|
||||
"src/SCS.cpp"
|
||||
"src/SCSCL.cpp"
|
||||
"src/SCSerial.cpp"
|
||||
INCLUDE_DIRS
|
||||
"src"
|
||||
REQUIRES
|
||||
driver
|
||||
esp_timer
|
||||
)
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user