初始化

lib/SCServo/examples/SCSCL/Broadcast/Broadcast.ino

@@ -0,0 +1,32 @@
+/*
+The broadcast write example was tested in SCS15, 
+and if testing other models of SCS series rudders please change the 
+appropriate position, speed and delay parameters.
+*/
+
+// the uart used to control servos.
+// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
+#define S_RXD 18
+#define S_TXD 19
+
+
+#include <SCServo.h>
+
+SCSCL sc;
+
+void setup()
+{
+  Serial.begin(115200);
+  Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
+  sc.pSerial = &Serial1;
+  delay(1000);
+}
+
+void loop()
+{
+  sc.WritePos(0xfe, 1000, 0, 1500);//All servos move at the max speed=1500, move to Position=1000
+  delay(754);//[(P1-P0)/V]*1000+100
+  
+  sc.WritePos(0xfe, 20, 0, 1500);//All servos move at the max speed=1500, move to Position=20
+  delay(754);//[(P1-P0)/V]*1000+100
+}

lib/SCServo/examples/SCSCL/FeedBack/FeedBack.ino

@@ -0,0 +1,122 @@
+/*
+回读所有舵机反馈参数:位置、速度、负载、电压、温度、移动状态；
+FeedBack函数回读舵机参数于缓冲区，Readxxx(-1)函数返回缓冲区中相应的舵机状态；
+函数Readxxx(ID)，ID=-1返回FeedBack缓冲区参数；ID>=0，通过读指令直接返回指定ID舵机状态,
+无需调用FeedBack函数。
+Read back all feedback parameters: position, speed, load, voltage, temperature, movement status;
+The FeedBack function reads back the servo parameters in the buffer, and the Readxxx (-1) function returns the corresponding servo state in the buffer;
+Function Readxxx (ID), ID=1 returns the FeedBack buffer parameter; ID > 0, and directly returns the specified ID rudder state by reading the instruction.
+There is no need to call the FeedBack function.
+*/
+
+// the uart used to control servos.
+// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
+#define S_RXD 18
+#define S_TXD 19
+
+#include <SCServo.h>
+
+SCSCL sc;
+
+void setup()
+{
+  Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
+  Serial.begin(115200);
+  sc.pSerial = &Serial1;
+  delay(1000);
+}
+
+void loop()
+{
+  int Pos;
+  int Speed;
+  int Load;
+  int Voltage;
+  int Temper;
+  int Move;
+  if(sc.FeedBack(1)!=-1){
+    Pos = sc.ReadPos(-1);
+    Speed = sc.ReadSpeed(-1);
+    Load = sc.ReadLoad(-1);
+    Voltage = sc.ReadVoltage(-1);
+    Temper = sc.ReadTemper(-1);
+    Move = sc.ReadMove(-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);
+    delay(10);
+  }else{
+    Serial.println("FeedBack err");
+    delay(500);
+  }
+  
+  Pos = sc.ReadPos(1);
+  if(Pos!=-1){
+    Serial.print("Servo position:");
+    Serial.println(Pos, DEC);
+    delay(10);
+  }else{
+    Serial.println("read position err");
+    delay(500);
+  }
+  
+  Voltage = sc.ReadVoltage(1);
+  if(Voltage!=-1){
+	  Serial.print("Servo Voltage:");
+    Serial.println(Voltage, DEC);
+    delay(10);
+  }else{
+    Serial.println("read Voltage err");
+    delay(500);
+  }
+  
+  Temper = sc.ReadTemper(1);
+  if(Temper!=-1){
+    Serial.print("Servo temperature:");
+    Serial.println(Temper, DEC);
+    delay(10);
+  }else{
+    Serial.println("read temperature err");
+    delay(500);    
+  }
+
+  Speed = sc.ReadSpeed(1);
+  if(Speed!=-1){
+    Serial.print("Servo Speed:");
+    Serial.println(Speed, DEC);
+    delay(10);
+  }else{
+    Serial.println("read Speed err");
+    delay(500);    
+  }
+  
+  Load = sc.ReadLoad(1);
+  if(Load!=-1){
+    Serial.print("Servo Load:");
+    Serial.println(Load, DEC);
+    delay(10);
+  }else{
+    Serial.println("read Load err");
+    delay(500);    
+  }
+
+  Move = sc.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();
+}

lib/SCServo/examples/SCSCL/Ping/Ping.ino

@@ -0,0 +1,40 @@
+/*
+Ping() to test a servo on the bus is ready.
+*/
+
+// the uart used to control servos.
+// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
+#define S_RXD 18
+#define S_TXD 19
+
+#include <SCServo.h>
+
+SCSCL sc;
+
+int TEST_ID = 3;
+
+int LEDpin = 13;
+void setup()
+{
+  pinMode(LEDpin,OUTPUT);
+  digitalWrite(LEDpin, HIGH);
+  Serial.begin(115200);
+  Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
+  sc.pSerial = &Serial1;
+  delay(1000);
+}
+
+void loop()
+{
+  int ID = sc.Ping(TEST_ID);
+  if(ID!=-1){
+    digitalWrite(LEDpin, LOW);
+    Serial.print("Servo ID:");
+    Serial.println(ID, DEC);
+    delay(100);
+  }else{
+    Serial.println("Ping servo ID error!");
+    digitalWrite(LEDpin, HIGH);
+    delay(2000);
+  }
+}

lib/SCServo/examples/SCSCL/ProgramEprom/ProgramEprom.ino

@@ -0,0 +1,31 @@
+/*
+example for changing ID.
+*/
+
+// the uart used to control servos.
+// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
+#define S_RXD 18
+#define S_TXD 19
+
+#include <SCServo.h>
+
+SCSCL sc;
+
+int ID_ChangeFrom = 1;
+int ID_Changeto   = 2;
+
+void setup()
+{
+  Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
+  sc.pSerial = &Serial1;
+  delay(1000);
+
+  sc.unLockEprom(ID_ChangeFrom);//unlock EPROM-SAFE
+  sc.writeByte(ID_ChangeFrom, SCSCL_ID, ID_Changeto);//ID
+  sc.LockEprom(ID_Changeto);// EPROM-SAFE locked
+}
+
+void loop()
+{
+
+}

lib/SCServo/examples/SCSCL/RegWritePos/RegWritePos.ino

@@ -0,0 +1,35 @@
+/*
+The RegWritePos example passed the test in SCS15, 
+and if testing other models of SCS series servos
+please change the appropriate position, speed and delay parameters.
+*/
+
+// the uart used to control servos.
+// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
+#define S_RXD 18
+#define S_TXD 19
+
+#include <SCServo.h>
+
+SCSCL sc;
+
+void setup()
+{
+  Serial.begin(115200);
+  Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
+  sc.pSerial = &Serial1;
+  delay(1000);
+}
+
+void loop()
+{
+  sc.RegWritePos(1, 1000, 0, 1500);//Servo(ID1) moves at max speed=1500, moves to position=1000.
+  sc.RegWritePos(2, 1000, 0, 1500);//Servo(ID2) moves at max speed=1500, moves to position=1000.
+  sc.RegWriteAction();//Start moving.
+  delay(754);//[(P1-P0)/V]*1000+100
+  
+  sc.RegWritePos(1, 20, 0, 1500);//Servo(ID1) moves at max speed=1500, moves to position=20.
+  sc.RegWritePos(2, 20, 0, 1500);//Servo(ID2) moves at max speed=1500, moves to position=20.
+  sc.RegWriteAction();//Start moving.
+  delay(754);//[(P1-P0)/V]*1000+100
+}

lib/SCServo/examples/SCSCL/SyncWritePos/SyncWritePos.ino

@@ -0,0 +1,44 @@
+/*
+The SyncWritePos example passed the test in SCS15, 
+and if testing other models of SCS series servos
+please change the appropriate position, speed and delay parameters.
+*/
+
+// the uart used to control servos.
+// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
+#define S_RXD 18
+#define S_TXD 19
+
+#include <SCServo.h>
+
+SCSCL sc;
+
+byte ID[2];
+u16 Position[2];
+u16 Speed[2];
+
+void setup()
+{
+  Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
+  sc.pSerial = &Serial1;
+  delay(1000);
+  ID[0] = 1;
+  ID[1] = 2;
+}
+
+void loop()
+{
+  Position[0] = 1000;
+  Position[1] = 1000;
+  Speed[0] = 1500;
+  Speed[1] = 1500;
+  sc.SyncWritePos(ID, 2, Position, 0, Speed);//Servo((ID1/ID2)) moves at max speed=1500, moves to position=1000.
+  delay(754);//[(P1-P0)/V]*1000+100
+
+  Position[0] = 20;
+  Position[1] = 20;
+  Speed[0] = 1500;
+  Speed[1] = 1500;
+  sc.SyncWritePos(ID, 2, Position, 0, Speed);//Servo((ID1/ID2)) moves at max speed=1500, moves to position=20.
+  delay(754);//[(P1-P0)/V]*1000+100
+}

lib/SCServo/examples/SCSCL/WritePos/WritePos.ino

@@ -0,0 +1,31 @@
+/*
+The normal write example passed the test in SCS15, 
+and if testing other models of SCS series servos
+please change the appropriate position, speed and delay parameters.
+*/
+
+// the uart used to control servos.
+// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
+#define S_RXD 18
+#define S_TXD 19
+
+#include <SCServo.h>
+
+SCSCL sc;
+
+void setup()
+{
+  Serial.begin(115200);
+  Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
+  sc.pSerial = &Serial1;
+  delay(1000);
+}
+
+void loop()
+{
+  sc.WritePos(1, 1000, 0, 1500);//Servo(ID1) moves at max speed=1500, moves to position=1000.
+  delay(754);//[(P1-P0)/V]*1000+100
+  
+  sc.WritePos(1, 20, 0, 1500);//Servo(ID1) moves at max speed=1500, moves to position=20.
+  delay(754);//[(P1-P0)/V]*1000+100
+}

lib/SCServo/examples/STSCL/CalibrationOfs/CalibrationOfs.ino

@@ -0,0 +1,25 @@
+/*
+set the current position as middle.
+*/
+
+#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
+#define S_TXD 19
+
+void setup()
+{
+  Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
+  st.pSerial = &Serial1;
+  delay(1000);
+}
+
+void loop()
+{
+  st.CalibrationOfs(1);
+  while(1);
+}

lib/SCServo/examples/STSCL/FeedBack/FeedBack.ino

@@ -0,0 +1,135 @@
+/*
+回读所有舵机反馈参数:位置、速度、负载、电压、温度、移动状态、电流；
+FeedBack函数回读舵机参数于缓冲区，Readxxx(-1)函数返回缓冲区中相应的舵机状态；
+函数Readxxx(ID)，ID=-1返回FeedBack缓冲区参数；ID>=0，通过读指令直接返回指定ID舵机状态,
+无需调用FeedBack函数。
+Read back all feedback parameters: position, speed, load, voltage, temperature, movement status;
+The FeedBack function reads back the servo parameters in the buffer, and the Readxxx (-1) function returns the corresponding servo state in the buffer;
+Function Readxxx (ID), ID=1 returns the FeedBack buffer parameter; ID > 0, and directly returns the specified ID rudder state by reading the instruction.
+There is no need to call the FeedBack function.
+*/
+
+// the uart used to control servos.
+// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
+#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();
+}

lib/SCServo/examples/STSCL/Ping/Ping.ino

@@ -0,0 +1,34 @@
+/*
+Ping the servo to check if it is ready.
+*/
+
+#include <SCServo.h>
+
+SMS_STS sms_sts;
+// the uart used to control servos.
+// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
+#define S_RXD 18
+#define S_TXD 19
+
+int TEST_ID = 3;
+
+void setup()
+{
+  Serial.begin(115200);
+  Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
+  sms_sts.pSerial = &Serial1;
+  delay(1000);
+}
+
+void loop()
+{
+  int ID = sms_sts.Ping(TEST_ID);
+  if(ID!=-1){
+    Serial.print("Servo ID:");
+    Serial.println(ID, DEC);
+    delay(100);
+  }else{
+    Serial.println("Ping servo ID error!");
+    delay(2000);
+  }
+}

lib/SCServo/examples/STSCL/ProgramEprom/ProgramEprom.ino

@@ -0,0 +1,29 @@
+/*
+example for changing ID.
+*/
+
+#include <SCServo.h>
+
+SMS_STS sms_sts;
+// the uart used to control servos.
+// GPIO 18 - S_RXD, GPIO 19 - S_TXD, as default.
+#define S_RXD 18
+#define S_TXD 19
+
+int ID_ChangeFrom = 1;
+int ID_Changeto   = 2;
+
+void setup()
+{
+  Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
+  sms_sts.pSerial = &Serial1;
+  delay(1000);
+  sms_sts.unLockEprom(1);//unlock EPROM-SAFE
+  sms_sts.writeByte(ID_ChangeFrom, SMS_STS_ID, ID_Changeto);//ID
+  sms_sts.LockEprom(ID_Changeto);//EPROM-SAFE locked
+}
+
+void loop()
+{
+
+}

lib/SCServo/examples/STSCL/RegWritePos/RegWritePos.ino

@@ -0,0 +1,36 @@
+/*
+The RegWritePos example passed the test in ST3215 Servo, 
+and if testing other models of ST series servos
+please change the appropriate position, speed and delay parameters.
+*/
+
+#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
+#define S_TXD 19
+
+
+void setup()
+{
+  Serial.begin(115200);
+  Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
+  st.pSerial = &Serial1;
+  delay(1000);
+}
+
+void loop()
+{
+  st.RegWritePosEx(1, 4095, 3400, 50);//servo(ID1) speed=3400，acc=50，move to position=4095.
+  st.RegWritePosEx(2, 4095, 3400, 50);//servo(ID2) speed=3400，acc=50，move to position=4095.
+  st.RegWriteAction();
+  delay(3000);
+
+  st.RegWritePosEx(1, 0, 3400, 50);//servo(ID1) speed=3400，acc=50，move to position=0.
+  st.RegWritePosEx(2, 0, 3400, 50);//servo(ID2) speed=3400，acc=50，move to position=0.
+  st.RegWriteAction();
+  delay(3000);
+}

lib/SCServo/examples/STSCL/SyncWritePos/SyncWritePos.ino

@@ -0,0 +1,44 @@
+/*
+The SyncWritePos example passed the test in ST3215 Servo, 
+and if testing other models of ST series servos
+please change the appropriate position, speed and delay parameters.
+*/
+
+#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
+#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[1] = 2;
+  Speed[0] = 3400;
+  Speed[1] = 3400;
+  ACC[0] = 50;
+  ACC[1] = 50;
+}
+
+void loop()
+{
+  Position[0] = 3000;
+  Position[1] = 3000;
+  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);
+}

lib/SCServo/examples/STSCL/WritePos/WritePos.ino

@@ -0,0 +1,31 @@
+/*
+The normal write example passed the test in ST3215 Servo, 
+and if testing other models of ST series servos
+please change the appropriate position, speed and delay parameters.
+*/
+
+#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
+#define S_TXD 19
+
+void setup()
+{
+  Serial.begin(115200);
+  Serial1.begin(1000000, SERIAL_8N1, S_RXD, S_TXD);
+  st.pSerial = &Serial1;
+  delay(1000);
+}
+
+void loop()
+{
+  st.WritePosEx(1, 4095, 3400, 50);//servo(ID1) speed=3400，acc=50，move to position=4095.
+  delay(2000);
+  
+  st.WritePosEx(1, 2000, 1500, 50);//servo(ID1) speed=3400，acc=50，move to position=2000.
+  delay(2000);
+}