Hello!
The smart control servos are really cool, because they have individual ID's so they can be used in a BUS network. There is a readPosition and writePosition function, that's why I want to use them.
The servos come with an arduino library (SCServo.h). The communitcation between arduino and the servos is over RX and TX. Between there is a TTLinker (Half Duplex Asynchronous Communication).
Here is my problem:
I need to change the communication from serial to serial1 or serial2. I guess I need to change the library. I tried without success. If anybody has an idea, I'd be very happy.
Thanks already for your time!
Here are the libraries:
SCServo.cpp
-----------------
/*
* SCServo.cpp
* Series Control Servo
* Created on: 2014.10.15
* Author: Tony tan
*/
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#define printf(args) (Serial.write(args))
#else
#include "WProgram.h"
#define printf(args) (Serial.print(args,BYTE))
#endif
#include "SCServo.h"
SCServo::SCServo ()
{
}
u8 SCServo::EnableTorque(u8 ID, u8 Enable, u8 ReturnLevel)
{
int messageLength = 4;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_TORQUE_ENABLE);
printf(Enable);
printf((~(ID + messageLength + INST_WRITE + Enable + P_TORQUE_ENABLE))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
u8 SCServo:: WritePos(u8 ID, s16 position, s16 velocity, u8 ReturnLevel)
{
int messageLength = 7;
byte posL = position>>8;
byte posH = position&0xff;
byte velL = velocity>>8;
byte velH = velocity&0xff;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_GOAL_POSITION_L);
printf(posL);
printf(posH);
printf(velL);
printf(velH);
printf((~(ID + messageLength + INST_WRITE + P_GOAL_POSITION_L + posL + posH + velL + velH))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
u8 SCServo:: RegWritePos(u8 ID, s16 position, s16 velocity, u8 ReturnLevel)
{
int messageLength = 7;
byte posL = position>>8;
byte posH = position&0xff;
byte velL = velocity>>8;
byte velH = velocity&0xff;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_REG_WRITE);
printf(P_GOAL_POSITION_L);
printf(posL);
printf(posH);
printf(velL);
printf(velH);
printf((~(ID + messageLength + INST_REG_WRITE + P_GOAL_POSITION_L + posL + posH + velL + velH))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
void SCServo:: RegWriteAction()
{
int messageLength = 2;
byte ID = 0xFE;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_ACTION);
printf((~(ID + messageLength + INST_ACTION))&0xFF);
}
u8 SCServo:: ReadBuf(u8 len, u8 *buf)
{
u16 n = 0;
u8 size = 0;
u8 ComData;
while(n<TIMEOUT)
{
if(Serial.available())
{
if(buf)
buf[size] = Serial.read();
else
ComData = Serial.read();
size++;
if(size>=len)
break;
n = 0;
}
n++;
}
return size;
}
s16 SCServo:: ReadPos(u8 ID)
{
u8 buf[8];
u8 size;
u16 pos;
memset(buf,0,sizeof(buf));
printf(startByte);
printf(startByte);
printf(ID);
printf(4);
printf(INST_READ);
printf(P_PRESENT_POSITION_L);
printf(2);
printf((~(ID + 4 + INST_READ + P_PRESENT_POSITION_L + 2))&0xFF);
size = ReadBuf(8, buf);
if(size<8)
return -1;
pos = buf[5];
pos <<= 8;
pos |= buf[6];
return (s16)pos;
}
void SCServo:: SyncWritePos(u8 ID[], u8 IDN, s16 position, s16 velocity)
{
int messageLength = 5*IDN+4;
u8 Sum = 0;
byte posL = position>>8;
byte posH = position&0xff;
byte velL = velocity>>8;
byte velH = velocity&0xff;
printf(startByte);
printf(startByte);
printf(0xfe);
printf(messageLength);
printf(INST_SYNC_WRITE);
printf(P_GOAL_POSITION_L);
printf(4);
Sum = 0xfe + messageLength + INST_SYNC_WRITE + P_GOAL_POSITION_L + 4;
int i;
for(i=0; i<IDN; i++)
{
printf(ID);
printf(posL);
printf(posH);
printf(velL);
printf(velH);
Sum += ID + posL + posH + velL + velH;
}
printf((~Sum)&0xFF);
}
u8 SCServo:: WriteID(u8 oldID, u8 newID, u8 ReturnLevel)
{
int messageLength = 4;
printf(startByte);
printf(startByte);
printf(oldID);
printf(messageLength);
printf(INST_WRITE);
printf(P_ID);
printf(newID);
printf((~(oldID + messageLength + INST_WRITE + newID + P_ID))&0xFF);
if(oldID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
u8 SCServo:: WriteLimitAngle(u8 ID, u16 MinAngel, u16 MaxAngle, u8 ReturnLevel)
{
int messageLength = 7;
byte MinAL = MinAngel>>8;
byte MinAH = MinAngel&0xff;
byte MaxAL = MaxAngle>>8;
byte MaxAH = MaxAngle&0xff;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_MIN_ANGLE_LIMIT_L);
printf(MinAL);
printf(MinAH);
printf(MaxAL);
printf(MaxAH);
printf((~(ID + messageLength + INST_WRITE + P_MIN_ANGLE_LIMIT_L + MinAL + MinAH + MaxAL + MaxAH))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
u8 SCServo:: WriteLimitTroque(u8 ID, u16 MaxTroque, u8 ReturnLevel)
{
int messageLength = 5;
byte MaxTL = MaxTroque>>8;
byte MaxTH = MaxTroque&0xff;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_MAX_TORQUE_L);
printf(MaxTL);
printf(MaxTH);
printf((~(ID + messageLength + INST_WRITE + P_MAX_TORQUE_L + MaxTL + MaxTH))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
u8 SCServo:: WritePunch(u8 ID, u16 Punch, u8 ReturnLevel)
{
int messageLength = 5;
byte PunchL = Punch>>8;
byte PunchH = Punch&0xff;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_PUNCH_L);
printf(PunchL);
printf(PunchH);
printf((~(ID + messageLength + INST_WRITE + P_PUNCH_L + PunchL + PunchH))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
u8 SCServo:: WriteBaund(u8 ID, u8 Baund, u8 ReturnLevel)
{
int messageLength = 4;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_BAUD_RATE);
printf(Baund);
printf((~(ID + messageLength + INST_WRITE + P_BAUD_RATE + Baund))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
u8 SCServo:: WriteComplianceMrgin(u8 ID, u8 CCW, u8 CW, u8 ReturnLevel)
{
int messageLength = 5;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_CW_COMPLIANCE_MARGIN);
printf(CW);
printf(CCW);
printf((~(ID + messageLength + INST_WRITE + P_CW_COMPLIANCE_MARGIN + CW + CCW))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
u8 SCServo:: LockEprom(u8 ID, u8 Enable, u8 ReturnLevel)
{
int messageLength = 4;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_LOCK);
printf(Enable);
printf((~(ID + messageLength + INST_WRITE + P_LOCK + Enable))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
u8 SCServo:: WritePID(u8 ID, u8 P, u8 I, u8 D, u8 ReturnLevel)
{
int messageLength = 6;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_COMPLIANCE_P);
printf(P);
printf(D);
printf(I);
printf((~(ID + messageLength + INST_WRITE + P_COMPLIANCE_P + P + D + I))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
u8 SCServo:: WriteSpe(u8 ID, s16 velocity, u8 ReturnLevel)
{
int messageLength = 5;
u16 vel = velocity;
if(velocity<0)
{
vel = -velocity;
vel |= (1<<10);
}
byte velL = vel>>8;
byte velH = vel&0xff;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_GOAL_SPEED_L);
printf(velL);
printf(velH);
printf((~(ID + messageLength + INST_WRITE + P_GOAL_SPEED_L + velL + velH))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
s16 SCServo:: ReadVoltage(u8 ID)
{
u8 buf[7];
u8 size;
s16 vol;
memset(buf,0,sizeof(buf));
printf(startByte);
printf(startByte);
printf(ID);
printf(4);
printf(INST_READ);
printf(P_PRESENT_VOLTAGE);
printf(1);
printf((~(ID + 4 + INST_READ + P_PRESENT_VOLTAGE + 1))&0xFF);
size = ReadBuf(7, buf);
if(size<7)
return -1;
vol = buf[5];
return vol;
}
s16 SCServo:: ReadTemper(u8 ID)
{
u8 buf[7];
u8 size;
s16 temper;
memset(buf,0,sizeof(buf));
printf(startByte);
printf(startByte);
printf(ID);
printf(4);
printf(INST_READ);
printf(P_PRESENT_TEMPERATURE);
printf(1);
printf((~(ID + 4 + INST_READ + P_PRESENT_TEMPERATURE + 1))&0xFF);
size = ReadBuf(7, buf);
if(size<7)
return -1;
temper = buf[5];
return temper;
}
------------
SCServo.h
------------
/*
* SCServo.h
* Series Control Servo
* Created on: 2014.10.15
* Author: Tony tan
*/
#ifndef _SCSERVO_h_
#define _SCSERVO_h_
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#define s8 char
#define u8 unsigned char
#define u16 unsigned short
#define s16 short
#define u32 unsigned long
#define s32 long
class SCServo{
public:
SCServo();
u8 EnableTorque(u8 ID, u8 Enable, u8 ReturnLevel=2);
u8 WritePos(u8 ID, s16 position, s16 velocity, u8 ReturnLevel=2);
u8 RegWritePos(u8 ID, s16 position, s16 velocity, u8 ReturnLevel=2);
s16 ReadPos(u8 ID);
s16 ReadVoltage(u8 ID);
s16 ReadTemper(u8 ID);
void RegWriteAction();
void SyncWritePos(u8 ID[], u8 IDN, s16 position, s16 velocity);
u8 WriteID(u8 oldID, u8 newID, u8 ReturnLevel=2);
u8 WriteLimitAngle(u8 ID, u16 MinAngel, u16 MaxAngle, u8 ReturnLevel=2);
u8 WriteLimitTroque(u8 ID, u16 MaxTroque, u8 ReturnLevel=2);
u8 WritePunch(u8 ID, u16 Punch, u8 ReturnLevel=2);
u8 WriteBaund(u8 ID, u8 Baund, u8 ReturnLevel=2);
u8 WriteComplianceMrgin(u8 ID, u8 CCW, u8 CW, u8 ReturnLevel=2);
u8 WritePID(u8 ID, u8 P, u8 I, u8 D, u8 ReturnLevel=2);
u8 WriteSpe(u8 ID, s16 velocity, u8 ReturnLevel=2);
u8 LockEprom(u8 ID, u8 Enable, u8 ReturnLevel=2);
private:
u8 ReadBuf(u8 len, u8 *buf=NULL);
#define startByte 0xFF
#define TIMEOUT 2000//TIMEOUT 2000
#define B_1M 0
#define B_0_5M 1
#define B_250K 2
#define B_128K 3
#define B_115200 4
#define B_76800 5
#define B_57600 6
#define B_38400 7
//register Address
#define P_MODEL_NUMBER_L 0
#define P_MODEL_NUMBER_H 1
#define P_VERSION_L 3
#define P_VERSION_H 4
#define P_ID 5
#define P_BAUD_RATE 6
#define P_RETURN_DELAY_TIME 7
#define P_RETURN_LEVEL 8
#define P_MIN_ANGLE_LIMIT_L 9
#define P_MIN_ANGLE_LIMIT_H 10
#define P_MAX_ANGLE_LIMIT_L 11
#define P_MAX_ANGLE_LIMIT_H 12
#define P_LIMIT_TEMPERATURE 13
#define P_MAX_LIMIT_VOLTAGE 14
#define P_MIN_LIMIT_VOLTAGE 15
#define P_MAX_TORQUE_L 16
#define P_MAX_TORQUE_H 17
#define P_ALARM_LED 18
#define P_ALARM_SHUTDOWN 19
#define P_COMPLIANCE_P 21
#define P_COMPLIANCE_D 22
#define P_COMPLIANCE_I 23
#define P_PUNCH_L 24
#define P_PUNCH_H 25
#define P_CW_COMPLIANCE_MARGIN 26
#define P_CCW_COMPLIANCE_MARGIN 27
#define P_TORQUE_ENABLE (31)
#define P_LED (32)
#define P_GOAL_POSITION_L (33)
#define P_GOAL_POSITION_H (34)
#define P_GOAL_SPEED_L (35)
#define P_GOAL_SPEED_H (36)
#define P_LOCK (37)
#define P_PRESENT_POSITION_L (41)
#define P_PRESENT_POSITION_H (42)
#define P_PRESENT_SPEED_L (43)
#define P_PRESENT_SPEED_H (44)
#define P_PRESENT_LOAD_L (45)
#define P_PRESENT_LOAD_H (46)
#define P_PRESENT_VOLTAGE (47)
#define P_PRESENT_TEMPERATURE (48)
#define P_REGISTERED_INSTRUCTION (49)
#define P_ERROR (50)
#define P_MOVING (51)
//Instruction:
#define INST_PING 0x01
#define INST_READ 0x02
#define INST_WRITE 0x03
#define INST_REG_WRITE 0x04
#define INST_ACTION 0x05
#define INST_RESET 0x06
#define INST_SYNC_WRITE 0x83
};
#endif
-----------
This is one example:
writePosition.ino
-----------
#include <SCServo.h>
SCServo SERVO;
void setup()
{
Serial.begin(1000000);//init Serial baudrate
delay(500);
SERVO.EnableTorque(4, 1);
SERVO.EnableTorque(6, 1);
}
void loop()
{
SERVO.WritePos(4, 0x02FF, 100);// Servo ID:1, rotate to the position:0x2FF
SERVO.WritePos(6, 0x02FF, 100);// Servo ID:2, rotate to the position:0x2FF
delay(8000);
SERVO.WritePos(4,0x0000, 75);// Servo ID:1, rotate to the position:0x000
SERVO.WritePos(6, 0x0000, 75);// Servo ID:1, rotate to the position:0x000
delay(6000);
SERVO.WritePos(4, 0x02FF, 50);// Servo ID:1, rotate to the position:0x2FF
SERVO.WritePos(6, 0x02FF, 50);// Servo ID:2, rotate to the position:0x2FF
delay(4000);
SERVO.WritePos(4, 0x0000, 25);// Servo ID:1, rotate to the position:0x000
SERVO.WritePos(6, 0x0000, 25);// Servo ID:2, rotate to the position:0x000
delay(2000);
}
----------
With kind regards,
Johannes
The smart control servos are really cool, because they have individual ID's so they can be used in a BUS network. There is a readPosition and writePosition function, that's why I want to use them.
The servos come with an arduino library (SCServo.h). The communitcation between arduino and the servos is over RX and TX. Between there is a TTLinker (Half Duplex Asynchronous Communication).
Here is my problem:
I need to change the communication from serial to serial1 or serial2. I guess I need to change the library. I tried without success. If anybody has an idea, I'd be very happy.
Thanks already for your time!
Here are the libraries:
SCServo.cpp
-----------------
/*
* SCServo.cpp
* Series Control Servo
* Created on: 2014.10.15
* Author: Tony tan
*/
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#define printf(args) (Serial.write(args))
#else
#include "WProgram.h"
#define printf(args) (Serial.print(args,BYTE))
#endif
#include "SCServo.h"
SCServo::SCServo ()
{
}
u8 SCServo::EnableTorque(u8 ID, u8 Enable, u8 ReturnLevel)
{
int messageLength = 4;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_TORQUE_ENABLE);
printf(Enable);
printf((~(ID + messageLength + INST_WRITE + Enable + P_TORQUE_ENABLE))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
u8 SCServo:: WritePos(u8 ID, s16 position, s16 velocity, u8 ReturnLevel)
{
int messageLength = 7;
byte posL = position>>8;
byte posH = position&0xff;
byte velL = velocity>>8;
byte velH = velocity&0xff;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_GOAL_POSITION_L);
printf(posL);
printf(posH);
printf(velL);
printf(velH);
printf((~(ID + messageLength + INST_WRITE + P_GOAL_POSITION_L + posL + posH + velL + velH))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
u8 SCServo:: RegWritePos(u8 ID, s16 position, s16 velocity, u8 ReturnLevel)
{
int messageLength = 7;
byte posL = position>>8;
byte posH = position&0xff;
byte velL = velocity>>8;
byte velH = velocity&0xff;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_REG_WRITE);
printf(P_GOAL_POSITION_L);
printf(posL);
printf(posH);
printf(velL);
printf(velH);
printf((~(ID + messageLength + INST_REG_WRITE + P_GOAL_POSITION_L + posL + posH + velL + velH))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
void SCServo:: RegWriteAction()
{
int messageLength = 2;
byte ID = 0xFE;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_ACTION);
printf((~(ID + messageLength + INST_ACTION))&0xFF);
}
u8 SCServo:: ReadBuf(u8 len, u8 *buf)
{
u16 n = 0;
u8 size = 0;
u8 ComData;
while(n<TIMEOUT)
{
if(Serial.available())
{
if(buf)
buf[size] = Serial.read();
else
ComData = Serial.read();
size++;
if(size>=len)
break;
n = 0;
}
n++;
}
return size;
}
s16 SCServo:: ReadPos(u8 ID)
{
u8 buf[8];
u8 size;
u16 pos;
memset(buf,0,sizeof(buf));
printf(startByte);
printf(startByte);
printf(ID);
printf(4);
printf(INST_READ);
printf(P_PRESENT_POSITION_L);
printf(2);
printf((~(ID + 4 + INST_READ + P_PRESENT_POSITION_L + 2))&0xFF);
size = ReadBuf(8, buf);
if(size<8)
return -1;
pos = buf[5];
pos <<= 8;
pos |= buf[6];
return (s16)pos;
}
void SCServo:: SyncWritePos(u8 ID[], u8 IDN, s16 position, s16 velocity)
{
int messageLength = 5*IDN+4;
u8 Sum = 0;
byte posL = position>>8;
byte posH = position&0xff;
byte velL = velocity>>8;
byte velH = velocity&0xff;
printf(startByte);
printf(startByte);
printf(0xfe);
printf(messageLength);
printf(INST_SYNC_WRITE);
printf(P_GOAL_POSITION_L);
printf(4);
Sum = 0xfe + messageLength + INST_SYNC_WRITE + P_GOAL_POSITION_L + 4;
int i;
for(i=0; i<IDN; i++)
{
printf(ID);
printf(posL);
printf(posH);
printf(velL);
printf(velH);
Sum += ID + posL + posH + velL + velH;
}
printf((~Sum)&0xFF);
}
u8 SCServo:: WriteID(u8 oldID, u8 newID, u8 ReturnLevel)
{
int messageLength = 4;
printf(startByte);
printf(startByte);
printf(oldID);
printf(messageLength);
printf(INST_WRITE);
printf(P_ID);
printf(newID);
printf((~(oldID + messageLength + INST_WRITE + newID + P_ID))&0xFF);
if(oldID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
u8 SCServo:: WriteLimitAngle(u8 ID, u16 MinAngel, u16 MaxAngle, u8 ReturnLevel)
{
int messageLength = 7;
byte MinAL = MinAngel>>8;
byte MinAH = MinAngel&0xff;
byte MaxAL = MaxAngle>>8;
byte MaxAH = MaxAngle&0xff;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_MIN_ANGLE_LIMIT_L);
printf(MinAL);
printf(MinAH);
printf(MaxAL);
printf(MaxAH);
printf((~(ID + messageLength + INST_WRITE + P_MIN_ANGLE_LIMIT_L + MinAL + MinAH + MaxAL + MaxAH))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
u8 SCServo:: WriteLimitTroque(u8 ID, u16 MaxTroque, u8 ReturnLevel)
{
int messageLength = 5;
byte MaxTL = MaxTroque>>8;
byte MaxTH = MaxTroque&0xff;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_MAX_TORQUE_L);
printf(MaxTL);
printf(MaxTH);
printf((~(ID + messageLength + INST_WRITE + P_MAX_TORQUE_L + MaxTL + MaxTH))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
u8 SCServo:: WritePunch(u8 ID, u16 Punch, u8 ReturnLevel)
{
int messageLength = 5;
byte PunchL = Punch>>8;
byte PunchH = Punch&0xff;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_PUNCH_L);
printf(PunchL);
printf(PunchH);
printf((~(ID + messageLength + INST_WRITE + P_PUNCH_L + PunchL + PunchH))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
u8 SCServo:: WriteBaund(u8 ID, u8 Baund, u8 ReturnLevel)
{
int messageLength = 4;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_BAUD_RATE);
printf(Baund);
printf((~(ID + messageLength + INST_WRITE + P_BAUD_RATE + Baund))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
u8 SCServo:: WriteComplianceMrgin(u8 ID, u8 CCW, u8 CW, u8 ReturnLevel)
{
int messageLength = 5;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_CW_COMPLIANCE_MARGIN);
printf(CW);
printf(CCW);
printf((~(ID + messageLength + INST_WRITE + P_CW_COMPLIANCE_MARGIN + CW + CCW))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
u8 SCServo:: LockEprom(u8 ID, u8 Enable, u8 ReturnLevel)
{
int messageLength = 4;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_LOCK);
printf(Enable);
printf((~(ID + messageLength + INST_WRITE + P_LOCK + Enable))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
u8 SCServo:: WritePID(u8 ID, u8 P, u8 I, u8 D, u8 ReturnLevel)
{
int messageLength = 6;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_COMPLIANCE_P);
printf(P);
printf(D);
printf(I);
printf((~(ID + messageLength + INST_WRITE + P_COMPLIANCE_P + P + D + I))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
u8 SCServo:: WriteSpe(u8 ID, s16 velocity, u8 ReturnLevel)
{
int messageLength = 5;
u16 vel = velocity;
if(velocity<0)
{
vel = -velocity;
vel |= (1<<10);
}
byte velL = vel>>8;
byte velH = vel&0xff;
printf(startByte);
printf(startByte);
printf(ID);
printf(messageLength);
printf(INST_WRITE);
printf(P_GOAL_SPEED_L);
printf(velL);
printf(velH);
printf((~(ID + messageLength + INST_WRITE + P_GOAL_SPEED_L + velL + velH))&0xFF);
if(ID != 0xfe && ReturnLevel==2)
return ReadBuf(6);
return 0;
}
s16 SCServo:: ReadVoltage(u8 ID)
{
u8 buf[7];
u8 size;
s16 vol;
memset(buf,0,sizeof(buf));
printf(startByte);
printf(startByte);
printf(ID);
printf(4);
printf(INST_READ);
printf(P_PRESENT_VOLTAGE);
printf(1);
printf((~(ID + 4 + INST_READ + P_PRESENT_VOLTAGE + 1))&0xFF);
size = ReadBuf(7, buf);
if(size<7)
return -1;
vol = buf[5];
return vol;
}
s16 SCServo:: ReadTemper(u8 ID)
{
u8 buf[7];
u8 size;
s16 temper;
memset(buf,0,sizeof(buf));
printf(startByte);
printf(startByte);
printf(ID);
printf(4);
printf(INST_READ);
printf(P_PRESENT_TEMPERATURE);
printf(1);
printf((~(ID + 4 + INST_READ + P_PRESENT_TEMPERATURE + 1))&0xFF);
size = ReadBuf(7, buf);
if(size<7)
return -1;
temper = buf[5];
return temper;
}
------------
SCServo.h
------------
/*
* SCServo.h
* Series Control Servo
* Created on: 2014.10.15
* Author: Tony tan
*/
#ifndef _SCSERVO_h_
#define _SCSERVO_h_
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#define s8 char
#define u8 unsigned char
#define u16 unsigned short
#define s16 short
#define u32 unsigned long
#define s32 long
class SCServo{
public:
SCServo();
u8 EnableTorque(u8 ID, u8 Enable, u8 ReturnLevel=2);
u8 WritePos(u8 ID, s16 position, s16 velocity, u8 ReturnLevel=2);
u8 RegWritePos(u8 ID, s16 position, s16 velocity, u8 ReturnLevel=2);
s16 ReadPos(u8 ID);
s16 ReadVoltage(u8 ID);
s16 ReadTemper(u8 ID);
void RegWriteAction();
void SyncWritePos(u8 ID[], u8 IDN, s16 position, s16 velocity);
u8 WriteID(u8 oldID, u8 newID, u8 ReturnLevel=2);
u8 WriteLimitAngle(u8 ID, u16 MinAngel, u16 MaxAngle, u8 ReturnLevel=2);
u8 WriteLimitTroque(u8 ID, u16 MaxTroque, u8 ReturnLevel=2);
u8 WritePunch(u8 ID, u16 Punch, u8 ReturnLevel=2);
u8 WriteBaund(u8 ID, u8 Baund, u8 ReturnLevel=2);
u8 WriteComplianceMrgin(u8 ID, u8 CCW, u8 CW, u8 ReturnLevel=2);
u8 WritePID(u8 ID, u8 P, u8 I, u8 D, u8 ReturnLevel=2);
u8 WriteSpe(u8 ID, s16 velocity, u8 ReturnLevel=2);
u8 LockEprom(u8 ID, u8 Enable, u8 ReturnLevel=2);
private:
u8 ReadBuf(u8 len, u8 *buf=NULL);
#define startByte 0xFF
#define TIMEOUT 2000//TIMEOUT 2000
#define B_1M 0
#define B_0_5M 1
#define B_250K 2
#define B_128K 3
#define B_115200 4
#define B_76800 5
#define B_57600 6
#define B_38400 7
//register Address
#define P_MODEL_NUMBER_L 0
#define P_MODEL_NUMBER_H 1
#define P_VERSION_L 3
#define P_VERSION_H 4
#define P_ID 5
#define P_BAUD_RATE 6
#define P_RETURN_DELAY_TIME 7
#define P_RETURN_LEVEL 8
#define P_MIN_ANGLE_LIMIT_L 9
#define P_MIN_ANGLE_LIMIT_H 10
#define P_MAX_ANGLE_LIMIT_L 11
#define P_MAX_ANGLE_LIMIT_H 12
#define P_LIMIT_TEMPERATURE 13
#define P_MAX_LIMIT_VOLTAGE 14
#define P_MIN_LIMIT_VOLTAGE 15
#define P_MAX_TORQUE_L 16
#define P_MAX_TORQUE_H 17
#define P_ALARM_LED 18
#define P_ALARM_SHUTDOWN 19
#define P_COMPLIANCE_P 21
#define P_COMPLIANCE_D 22
#define P_COMPLIANCE_I 23
#define P_PUNCH_L 24
#define P_PUNCH_H 25
#define P_CW_COMPLIANCE_MARGIN 26
#define P_CCW_COMPLIANCE_MARGIN 27
#define P_TORQUE_ENABLE (31)
#define P_LED (32)
#define P_GOAL_POSITION_L (33)
#define P_GOAL_POSITION_H (34)
#define P_GOAL_SPEED_L (35)
#define P_GOAL_SPEED_H (36)
#define P_LOCK (37)
#define P_PRESENT_POSITION_L (41)
#define P_PRESENT_POSITION_H (42)
#define P_PRESENT_SPEED_L (43)
#define P_PRESENT_SPEED_H (44)
#define P_PRESENT_LOAD_L (45)
#define P_PRESENT_LOAD_H (46)
#define P_PRESENT_VOLTAGE (47)
#define P_PRESENT_TEMPERATURE (48)
#define P_REGISTERED_INSTRUCTION (49)
#define P_ERROR (50)
#define P_MOVING (51)
//Instruction:
#define INST_PING 0x01
#define INST_READ 0x02
#define INST_WRITE 0x03
#define INST_REG_WRITE 0x04
#define INST_ACTION 0x05
#define INST_RESET 0x06
#define INST_SYNC_WRITE 0x83
};
#endif
-----------
This is one example:
writePosition.ino
-----------
#include <SCServo.h>
SCServo SERVO;
void setup()
{
Serial.begin(1000000);//init Serial baudrate
delay(500);
SERVO.EnableTorque(4, 1);
SERVO.EnableTorque(6, 1);
}
void loop()
{
SERVO.WritePos(4, 0x02FF, 100);// Servo ID:1, rotate to the position:0x2FF
SERVO.WritePos(6, 0x02FF, 100);// Servo ID:2, rotate to the position:0x2FF
delay(8000);
SERVO.WritePos(4,0x0000, 75);// Servo ID:1, rotate to the position:0x000
SERVO.WritePos(6, 0x0000, 75);// Servo ID:1, rotate to the position:0x000
delay(6000);
SERVO.WritePos(4, 0x02FF, 50);// Servo ID:1, rotate to the position:0x2FF
SERVO.WritePos(6, 0x02FF, 50);// Servo ID:2, rotate to the position:0x2FF
delay(4000);
SERVO.WritePos(4, 0x0000, 25);// Servo ID:1, rotate to the position:0x000
SERVO.WritePos(6, 0x0000, 25);// Servo ID:2, rotate to the position:0x000
delay(2000);
}
----------
With kind regards,
Johannes
Last edited: