BioloidMessage.py

From IPRE Wiki

import serial
ser = serial.Serial("com1", 57600)

def makePacket(i):
    
    msb = i >> 8
    lsb = i & 0x00FF
    num = 255, 85, lsb, 255 - lsb, msb, 255 - msb
    msg = chr(255) + chr(85) + chr(lsb) + chr(255 - lsb) + chr(msb) + chr(255 - msb)

    return msg

def panTilt(p, t):
    # p, t are in degrees between -90 and 90.
    # convert those degrees to numbers between 256 and 768
    pan = degreesToServo(p)
    ser.write(makePacket(2))
    ser.write(makePacket(pan))

    tilt = degreesToServo(t)
    ser.write(makePacket(1))
    ser.write(makePacket(tilt))


def degreesToServo(d):
    # gives you a number between zero and 1
    f = (d + 90)/180.0
    print f

    # returns a number between 256 and 768 so that the servos can understand it
    return int(f * 512 + 256)