demo.py

Go to the documentation of this file.

#! /usr/bin/env python

# Software License Agreement (BSD License)
#
# Copyright (c) 2010, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of Willow Garage, Inc. nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

import bluetooth


def print_hex(data):    
    for i,s in enumerate(data):
        print "%02x" % ord(s),
        if i%16 == 15:
            print
    print

def print_hex_msg(data):
    count = 0
    for s in data:
        val = ord(s)
        if val == 2:
            count = 0
            print '\n\n'
        if val == 3:
            count = 0
            print '\n\n'
        print ("%02x" % val),
        count += 1
        if count == 16:
            count = 0
            print
    for s in data:
        val = ord(s)
        if val == 2:
            count = 0
            print '\n\n'
        if val == 3:
            count = 0
            print '\n\n'
        print ("%02x" % val),
        count += 1
        if count == 16:
            count = 0
            print

    print    

def convert_str(data):
    result = []
    for c in data:    
        result.append(ord(c))
    return result

def convert_hex(data):
    s = ""
    for val in data:
        s+=chr(val)
    return s

def hex_to_short(data):
    return (data[0] << 8) | data[1]

seqnum = 1
def make_msg(typ, body):
    global seqnum
    data = [ 0 for i in range(8)]
    length = len(body)+8  # header length (8) + body length
    data[0] = (length >> 8) & 0xFF
    data[1] = length & 0xFF
    data[2] = 0 #checksum placeholder    
    data[3] = (seqnum >> 8) & 0xFF
    data[4] = seqnum & 0xFF
    seqnum+=1
    data[5] = 0 #resend?
    data[6] = (typ >> 8) & 0xFF
    data[7] = (typ & 0xFF)
    data += body
    data[2] = (-sum(data)) & 0xff
    return [2] + data + [3]

def make_action_msg(action,speed,stops,do_blowout,mix_cycles):
    body = [ 0 for i in range(6) ] 
    body[0] = action
    body[1] = speed
    body[2] = (stops>>8)&0xff
    body[3] = stops&0xff
    body[4] = do_blowout
    body[5] = mix_cycles
    return make_msg(4,body)

def make_voyager_move(position):
    body = [ (position>>8)&0xff, position&0xff ]
    return make_msg(12,body)

class PipetteOutputHeader:
    def __init__(self,data):
        data = data[1:-2]
        self.length = (data[0] << 8) | (data[1])
        if (sum(data)&0xFF) != 0:
            print sum(data)
            print (sum(data) & 0xff)
            print "bad checksum"
            
        self.checksum = data[2]
        self.seqnum = (data[3] << 8) | (data[4])        
        self.resend = data[5]
        self.msg_type = (data[6] << 8) | (data[7])
        self.error_code = data[8]
        self.body = data[9:]
        print "Body = ", self.body

    def __str__(self):
        s = "PipetteOutputHeader\n"
        s += " length %d\n" % self.length
        s += " cksum  0x%02x\n" % self.checksum
        s += " seqnum %d\n" % self.seqnum
        s += " resend %d\n" % self.resend
        s += " type   %d\n" % self.msg_type
        s += " error  %d\n" % self.error_code
        s += " body   "
        for v in self.body:
            s += "%02x" % v
        s += '\n'
        return s

class PipetteInfoOutput:
    def __init__(self, hdr):
        self.hdr = hdr
        body = hdr.body
        print " owner : '%s'" % convert_hex(body[0:11+1])
        print " serial : '%s'" % convert_hex(body[12:23+1])
        print " fwmajor : ", hex_to_short(body[24:25+1])
        print " fwminor : ", hex_to_short(body[26:27+1])
        print " motorsteps millions : ", hex_to_short(body[58:61+1])
        print " motorsteps          : ", hex_to_short(body[62:65+1]) 

#print make_msg(1, [])
#print convert_hex([2, 0, 0, 1, 0, 0, 0, 0, 1, 3])

def send_msg(sock,data):
    bytes_sent = sock.send( convert_hex(data) )
    if bytes_sent != len(data):
        raise RuntimeError("Sent only %d of %d bytes"%(bytes_sent, len(data)))

def recv_msg(sock):
    data = []
    while (len(data) == 0) or (data[-1] != chr(3)):
        data += sock.recv(1024)
        #print "Recieved :",  convert_str(data)
        #print_hex(data)

    print "Recieved : %d bytes" % (len(data))
    print_hex(data)
    h = PipetteOutputHeader(convert_str(data))
    print h
    return h


bd_addr = "00:80:98:e6:2b:30"
port = 1

#data = make_msg(1, [])
#print data

sock=bluetooth.BluetoothSocket( bluetooth.RFCOMM )
sock.connect((bd_addr, port))
#bluetooth.set_packet_timeout(bd_addr, int(0))

print "Connected"
data = sock.recv(1024)
print "Recieved",  convert_str(data)

send_msg(sock, make_msg(1, []))
h = recv_msg(sock)
i = PipetteInfoOutput(h)


#def make_action_msg(action,speed,stops,do_blowout,mix_cycles):
#send_msg(sock,make_action_msg(7,50,0,1,0))
#recv_msg(sock)

#send_msg(sock,make_voyager_move(0))
#recv_msg(sock)
    
#send_msg(sock,make_voyager_move(1000))
#recv_msg(sock)


sock.close()