The following tutorial is based on the code which appears in the EmuBot folder of the GitHub repository. It provides a base level of code to communicate with the servos. Before continuing this tutorial you should also check out the Moving Wheels tutorial.
Introduction
This code will continue on from your moving wheels code so it is assumed that you already have a serial connection setup. Check out the Moving Wheels if you don’t know what that is.
jointMode
Just as we did for the moving of the wheels we are going to create a function called jointMode which will communicate to the OpenCM to setup the joints.
def jointMode(ID):
s.write('W'+'j'+chr(ID))
moveJoint
The moveJoint() function receives three values when called, the ID, the position you want to send it o and the speed. The comments in the code mention the limitations, however, you must be careful when moving the joints, if you move them to a point that they cannot move to then torque mode will kick in and the servo will lock up. This will be shown by an LED light on the servo and if you turn the servo on and off it will release again. The servo will receive a value between 0 and 1025 with the middle point being 512.
def moveJoint(ID, position, speed):
#move to position between 0-1024
#512 is 12 o'clock
#WRITE ID
s.write('W'+'p'+chr(ID))
#write position
position = int(position)
s.write(chr(int(position)%256))
s.write(chr(int(position)>>8))
#WRITE SPEED
velocity = int(speed)
s.write(chr(int(velocity)%256))
s.write(chr(int(velocity)>>8))
#READ POSITION
time.sleep(0.5)
print(readDxl(ID,"j"))
If you are finding that these values are not true you may need to unscrew the servo, position it to the centre point and screw the joint back into position.
Calling our functions
Now that we have prepared our nice blocks of codes which we can call to move particular servos.
#setup the servos as joints
jointMode(5)
jointMode(6)
jointMode(7)
#move the joints to a new position
moveJoint(5,512,100)
moveJoint(6,512,100)
moveJoint(7,512,100)