本产品是一款大扭矩的可编程串行总线伺服电机,采用了360°高精度的磁编码器,可以在360°范围内进行角度控制,转速最高可达106RPM。能够通过程序控制,将任意角度设置为伺服电机中位,也可以切换为连续转动的电机(速度闭环控制)/步进电机模式,内置加速度启停功能,使动作更加柔和。每个伺服电机上面有两个接口,可以将电机串联起来使用,理论上可以同时控制253个总线伺服电机并且每个电机都能够获取自己当前的角度、负载、电压、模式等信息。适用于机械手臂,六足机器人,人形机器人,轮式机器人等需要闭环控制的机器人项目。
同时我们为该型号伺服电机提供了开源的12自由度机器狗模型,你可以在最下面的资料-开源结构中下载该开源结构的模型和工程文件。
一般来说,我们是通过Arduino IDE编译上传程序后才能控制ST系列舵机,但是通过Arduino编译需要给Arduino安装各种依赖库后才能使用。因此,我们提供一款ESP32下载工具,使用这个工具,用户不需要下载其它依赖库以及Arduino IDE软件即可将程序下载至驱动板里。
虽然上面提供了一种简便上传程序的方法,但在这里还是会介绍使用Arduino IDE编译上传程序的方法。我们为总线舵机驱动板提供兼容ST3215-HS舵机的例程,可以通过向总线舵机驱动板中上传ServoDriverST开源程序(Arduino)来控制ST系列舵机,以下是具体方法:
https://dl.espressif.com/dl/package_esp32_index.json
注意:如果您需要添加多个开发板URL,那无需将ESP32开发板支持的URL删掉,可以直接将其他URL添加至另一行,默认显示是逗号分隔开URL的。例如:如果您需要添加ESP8266开发板的URL,直接添加至另一行,显示出来如下为:
https://dl.espressif.com/dl/package_esp32_index.json,http://arduino.esp8266.com/stable/package_esp8266com_index.json
C:\Users\username\AppData\Local\Arduino15username根据自己电脑的用户名变换,将解压的packages文件复制到Arduino15文件夹下
每个控制舵机的程序,都需要对舵机进行初始化,初始化后才可以使用。
#include <SCServo.h>
SMS_STS st;
void setup(){
Serial1.begin(1000000); //串口初始化,如果使用ESP32等设备也可以选择自定义串口
// Serial1.begin(1000000, SERIAL_8N1, RX, TX); // 自定义串口
st.pSerial = &Serial1;
while(!Serial1) {}
}
串联起来的舵机中,每个ID只能对应一个舵机,否则不能正常获取舵机反馈的信息。 当更改舵机ID时,尽量保证驱动板只与一个舵机相连接,ID会被永久保存在舵机中。
#include <SCServo.h>
SMS_STS st;
int ID_ChangeFrom = 1; // 要更改ID的舵机原本ID,出厂默认为1
int ID_Changeto = 2; // 新的ID
void setup(){
Serial1.begin(1000000);
st.pSerial = &Serial1;
while(!Serial1) {}
st.unLockEprom(ID_ChangeFrom); //解锁EPROM-SAFE
st.writeByte(ID_ChangeFrom, SMS_STS_ID, ID_Changeto);//更改ID
st.LockEprom(ID_Changeto); // EPROM-SAFE上锁
}
void loop(){
}
用于测试某一舵机是否正常连接。
#include <SCServo.h>
SMS_STS st;
int TEST_ID = 3; // 要测试的舵机ID
void setup()
{
Serial.begin(115200);
Serial1.begin(1000000, SERIAL_8N1, RX, TX); // 自定义串口
st.pSerial = &Serial1;
while(!Serial1) {}
}
void loop()
{
int ID = st.Ping(TEST_ID); //ping该ID的舵机,ping不通返回-1
if(ID!=-1){
Serial.print("Servo ID:");
Serial.println(ID, DEC);
delay(100);
}else{
Serial.println("Ping servo ID error!");
delay(2000);
}
}
可用于控制单独舵机转动。
#include <SCServo.h>
SMS_STS st;
void setup()
{
Serial1.begin(1000000);
st.pSerial = &Serial1;
while(!Serial1) {}
}
void loop()
{
st.WritePos(1, 1000, 1500, 50); // 控制ID为1的舵机以1500的速度转动到1000的位置,启停加速度50。
delay(754);//[(P1-P0)/V]*1000+100
st.WritePos(1, 20, 1500, 50); // 控制ID为1的舵机以1500的速度转动到20的位置,启停加速度50。
delay(754);//[(P1-P0)/V]*1000+100
}
可用于同时控制多个舵机(转动到不同的位置和不同的速度)。
#include <SCServo.h>
SMS_STS st;
// the uart used to control servos.
// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
#define S_RXD 18 //自定义串口的IO,如果不使用自定义串口下面的Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);需要改为Serial1.begin(1000000);
#define S_TXD 19
byte ID[2];
s16 Position[2];
u16 Speed[2];
byte ACC[2];
void setup()
{
Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
st.pSerial = &Serial1;
delay(1000);
ID[0] = 1; // 将需要控制的舵机ID保存到ID[]中。
ID[1] = 2; // 将需要控制的舵机ID保存到ID[]中。
Speed[0] = 3400; // 设置舵机速度,Speed[0]与上面的ID[0]的舵机对应。
Speed[1] = 3400; // 设置舵机速度,Speed[1]与上面的ID[1]的舵机对应。
ACC[0] = 50; // 设置启停加速度,数值越小加速度越小,最大可设置为150.
ACC[1] = 50;
}
void loop()
{
Position[0] = 3000; // 设置ID[0](这里是ID为1的)舵机的目标位置,范围为0-4095
Position[1] = 3000; // 设置ID[1](这里是ID为2的)舵机的目标位置,范围为0-4095
st.SyncWritePosEx(ID, 2, Position, Speed, ACC);//servo(ID1/ID2) speed=3400,acc=50,move to position=3000.
delay(2000);
Position[0] = 100;
Position[1] = 100;
st.SyncWritePosEx(ID, 2, Position, Speed, ACC);//servo(ID1/ID2) speed=3400,acc=50,move to position=100.
delay(2000);
}
#define S_RXD 18
#define S_TXD 19
#include <SCServo.h>
SMS_STS sms_sts;
void setup()
{
Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
Serial.begin(115200);
sms_sts.pSerial = &Serial1;
delay(1000);
}
void loop()
{
int Pos;
int Speed;
int Load;
int Voltage;
int Temper;
int Move;
int Current;
if(sms_sts.FeedBack(1)!=-1){
Pos = sms_sts.ReadPos(-1);
Speed = sms_sts.ReadSpeed(-1);
Load = sms_sts.ReadLoad(-1);
Voltage = sms_sts.ReadVoltage(-1);
Temper = sms_sts.ReadTemper(-1);
Move = sms_sts.ReadMove(-1);
Current = sms_sts.ReadCurrent(-1);
Serial.print("Position:");
Serial.println(Pos);
Serial.print("Speed:");
Serial.println(Speed);
Serial.print("Load:");
Serial.println(Load);
Serial.print("Voltage:");
Serial.println(Voltage);
Serial.print("Temper:");
Serial.println(Temper);
Serial.print("Move:");
Serial.println(Move);
Serial.print("Current:");
Serial.println(Current);
delay(10);
}else{
Serial.println("FeedBack err");
delay(500);
}
Pos = sms_sts.ReadPos(1); // 获得位置反馈
if(Pos!=-1){
Serial.print("Servo position:");
Serial.println(Pos, DEC);
delay(10);
}else{
Serial.println("read position err");
delay(500);
}
Voltage = sms_sts.ReadVoltage(1); // 获得电压反馈
if(Voltage!=-1){
Serial.print("Servo Voltage:");
Serial.println(Voltage, DEC);
delay(10);
}else{
Serial.println("read Voltage err");
delay(500);
}
Temper = sms_sts.ReadTemper(1); // 获得温度反馈
if(Temper!=-1){
Serial.print("Servo temperature:");
Serial.println(Temper, DEC);
delay(10);
}else{
Serial.println("read temperature err");
delay(500);
}
Speed = sms_sts.ReadSpeed(1); // 获得速度反馈
if(Speed!=-1){
Serial.print("Servo Speed:");
Serial.println(Speed, DEC);
delay(10);
}else{
Serial.println("read Speed err");
delay(500);
}
Load = sms_sts.ReadLoad(1); // 获得负载反馈
if(Load!=-1){
Serial.print("Servo Load:");
Serial.println(Load, DEC);
delay(10);
}else{
Serial.println("read Load err");
delay(500);
}
Current = sms_sts.ReadCurrent(1); // 获得电流反馈
if(Current!=-1){
Serial.print("Servo Current:");
Serial.println(Current, DEC);
delay(10);
}else{
Serial.println("read Current err");
delay(500);
}
Move = sms_sts.ReadMove(1); // 获得运动状态反馈
if(Move!=-1){
Serial.print("Servo Move:");
Serial.println(Move, DEC);
delay(10);
}else{
Serial.println("read Move err");
delay(500);
}
Serial.println();
}