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. This tutorial will explain the code necessary to get the wheels moving forward and backwards.
Introduction
As this point I’m assuming that your robot has been wired up correctly. If you are having issues with the wheels moving then it is worth checking the Rx/Tx connection between the Raspberry Pi and the OpenCM and also checking that the servo ID’s have been correctly setup.
Create a serial connection
First you must open a serial port to allow communication to the servos via the OpenCM board.
import serial
import time
s = serial.Serial() #create a serial port object
s.baudrate = 57600 #set the rate of communication on the port (in bits/second)
s.port = "/dev/ttyAMA0" #this is which port you are using
s.timeout = 3.0
s.open()
wheelMode
We must then create a function which will setup the particular servos as wheels NOT joints. This is a pre-defined method in the dynamixel which is configured on the OpenCM so all we are doing is sending some characters and an ID to the openCM and the code on the openCM will do the rest.
def wheelMode(ID):
s.write('W'+'w'+chr(ID))
the two characters and the ID number which is converted to a character also are sent via the serial port to the openCM. We are only able to send 8 bytes of data at a time.
moveWheel
We are now doing to create a function which will handle the moving of wheels. It is worth noting that you need to be cautions when moving any of the servos, start slow and then increase the speed gradually. Running the servos at full speed for a long time can result in overheating.
The servos can receive a value between 0 (which is stopped) and 999 (which is full speed ahead!).
def moveWheel(ID, speed):
s.write('W'+'s'+chr(ID))
#WRITE ADDRESS
addr = int(32)
s.write(chr(int(addr)%256))
s.write(chr(int(addr)>>8))
#WRITE SPEED
if speed >= 0:
#Wrapper for forward speed
speed = int(speed)
else:
#Wrapper for backward speed
speed = int(1024 + int(-speed))
s.write(chr(int(speed)%256))
s.write(chr(int(speed)>>8))
This block of code writes to the serial port multiple times. First it sends the instruction to the openCM that we are going to send a write command (‘W’) and it is related to speed (‘s’). We configure an address (’32’) this is a set control address which is defined by the dynamical library and can be found in the dynamixel documentation. We then send the speed. A little bit of maths is performed on the integers to ensure that when they are passed via serial they appear as intended.
Calling our functions
Now that we have prepared nice blocks of code which we can call to move particular servos as wheels we can start to call the functions.
#setup the servos as wheels
wheelMode(1)
wheelMode(2)
wheelMode(3)
wheelMode(4)
#move the wheels forward at a speed of 100
moveWheel(1, 100)
moveWheel(2, 100)
moveWheel(3, 100)
moveWheel(4, 100)
#use time to allow them to run for 3 seconds
time.sleep(3)
#move the wheels backwards at a speed of 100
moveWheel(1, -100)
moveWheel(2, -100)
moveWheel(3, -100)
moveWheel(4, -100)
#use time to allow them to run for 3 seconds
time.sleep(3)
#stop all of the servos by sending a speed of 0.
moveWheel(1, 0)
moveWheel(2, 0)
moveWheel(3, 0)
moveWheel(4, 0)