Servo Control in Python

Posted on 2011-03-24 01:45:21

The class below does a basic job of controlling servo motors using the Pololu Micro Maestro servo controller board.

import serial
class PololuMicroMaestro(object):
    def __init__(self, port= "/dev/ttyACM0"):
        self.ser = serial.Serial(port = port)
    def setAngle(self, channel, angle):
        """Set the target angle of the servo.  This is converted into "quarter microseconds", i.e., the pulse width necessary to get to that angle (and thus it's between 1.0ms and 2.0ms in increments of 0.25us).  Whew!"""
        minAngle = 0.0
        maxAngle = 180.0
        # these numbers, in quarter microseconds, taken from the code here:
        # http://forum.pololu.com/viewtopic.php?t=2380#p10697
        minTarget = 256.0
        maxTarget = 13120.0
        scaledValue = int((angle / ((maxAngle - minAngle) / (maxTarget - minTarget))) + minTarget)
        commandByte = chr(0x84)
        channelByte = chr(channel)
        lowTargetByte = chr(scaledValue & 0x7F)
        highTargetByte = chr((scaledValue >> 7) & 0x7F)
        command = commandByte + channelByte + lowTargetByte + highTargetByte
        self.ser.write(command)
        self.ser.flush()
    def setSpeed(self, channel, speed):
        """Set the speed of the given channel.  Speed is given in units of 0.25us / 10ms.  This means there is a range from 1 to 40000.  Getting a handle of what this _actually_ means in practice, in terms of the visual speed of the motors, will take a bit of work."""
        commandByte = chr(0x87)
        channelByte = chr(channel)
        lowTargetByte = chr(speed & 0x7F)
        highTargetByte = chr((speed >> 7) & 0x7F)
        command = commandByte + channelByte + lowTargetByte + highTargetByte
        self.ser.write(command)
        self.ser.flush()
    def setAcceleration(self, channel, accel):
        """Set the acceleration of this channel.  Value should be between 1 and 255.  A setting of 0 removes the acceleration limit."""
        commandByte = chr(0x89)
        channelByte = chr(channel)
        lowTargetByte = chr(accel)
        highTargetByte = chr(0x00)
        command = commandByte + channelByte + lowTargetByte + highTargetByte
        self.ser.write(command)
        self.ser.flush()
    def getPosition(self, channel):
        """Get the position of this servo.  Returned in units of us."""
        commandByte = chr(0x90)
        channelByte = chr(channel)
        command = commandByte + channelByte
        self.ser.write(command)
        lowByte = ord(self.ser.read(1))
        highByte = ord(self.ser.read(1))
        highByte = highByte << 8
        position = highByte + lowByte
        return (position / 4.0)
    def goHome(self):
        """Set all servos to home position."""
        self.ser.write(chr(0xA2))
    def close(self):
        self.ser.close()

Comments

Submit comment