From 3d875e0b7d29c59be6a65b13cd4e2b0d157b6744 Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Sat, 4 Apr 2015 20:20:29 +0200 Subject: [PATCH] added teleop, move_base for cmd_vel --- launch/teleop.launch | 13 +++ scripts/bootloader.py | 179 ++++++++++++++++++++++++++++++++++++++++++ scripts/i2c.py | 72 +++++++++++++++++ scripts/move_base.py | 46 +++++++++++ 4 files changed, 310 insertions(+) create mode 100644 launch/teleop.launch create mode 100755 scripts/bootloader.py create mode 100755 scripts/i2c.py create mode 100755 scripts/move_base.py diff --git a/launch/teleop.launch b/launch/teleop.launch new file mode 100644 index 0000000..60f4adf --- /dev/null +++ b/launch/teleop.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/scripts/bootloader.py b/scripts/bootloader.py new file mode 100755 index 0000000..2abcb93 --- /dev/null +++ b/scripts/bootloader.py @@ -0,0 +1,179 @@ +#!/usr/bin/env python +# -*- coding: iso-8859-15 -*- + +import struct +import sys +import socket +from optparse import OptionParser +from time import sleep, time +from i2c import i2c + +CMD_READ = 0x1 +CMD_ERASE = 0x2 +CMD_WRITE = 0x3 +CMD_ERASE_ALL = 0x5 +CMD_JUMP = 0x6 +CMD_INFO = 0x99 + +PAGESIZE=64 + +class bootloader: + def __init__(self, addr): + self.i2c_addr = addr + self.boot_addr = 0x0 + + if not self.identify(): + raise "Bootloader not running" + + def read_mem(self, addr, num): + self.run_cmd(CMD_READ, addr, num) + return self.read(num) + + def erase(self, addr): + self.run_cmd(CMD_ERASE) + + def erase_all(self): + self.run_cmd(CMD_ERASE_ALL) + + def __compare_memarea(self, addr, data): + mem_cmp = self.read_mem(addr, 64) + if mem_cmp != data: + print "Expected:", data.encode("hex") + print "Got: ", mem_cmp.encode("hex") + raise("Compare mismatch at 0x%x" % addr) + return + + def __program_memarea(self, addr, data): + self.run_cmd(CMD_WRITE, addr, 64, data) + + def compare(self, filename): + return self.__process_hex(filename, self.__compare_memarea) + + def program(self, filename): + return self.__process_hex(filename, self.__program_memarea) + + def __process_hex(self, filename, handle): + next_addr = None + buf = "" + lFirstRow = None + + f = open(filename, "r") + count=0 + for line in f: + if line[0] != ':': + raise("Bad line start character") + hex = line[1:].replace("\r\n", "") + data = hex.decode("hex") + num = ord(data[0]) + chksum = 0 + for c in data: + chksum+=ord(c) + if chksum % 256 != 0: + raise("Checksum error") + addr, typ, data, chksum = struct.unpack(">HB%ssB" % num, data[1:]) + + if typ == 0: # Data Record + count+=len(data) + if next_addr is not None: + if next_addr != addr: + raise "Gap in file" + buf_addr = addr-len(buf) + buf+=data + if len(buf) >= PAGESIZE: + if not lFirstRow: + # do the first as last one + lFirstRow = (buf_addr, buf[:PAGESIZE]) + else: + print "Addr 0x%x" % buf_addr + handle(buf_addr, buf[:PAGESIZE]) + buf = buf[PAGESIZE:] + elif typ == 3: # Start Segment Address Record + self.boot_addr = int(data.encode("hex"), 16) + elif typ == 1: # End of File Record + print "Addr (rest) 0x%x" % buf_addr + buf_addr+=PAGESIZE + diff = PAGESIZE-len(buf) + buf+=chr(0xff)*diff # fill with 0xff + handle(buf_addr, buf[:PAGESIZE]) + if lFirstRow: # was first + buf_addr = lFirstRow[0] + buf = lFirstRow[1] + print "Addr (First) 0x%x" % buf_addr + handle(buf_addr, buf) + else: + raise("Unknown type %d" % typ) + + next_addr = addr+num + print "Byte count:", count + f.close() + + def jump(self, addr): + self.run_cmd(CMD_JUMP) + + def wait_ping(self): + while(True): + try: + self.identify() + break + except: + sleep(1) + + def load(self, filename): + print "Erase..." + self.erase_all() + self.wait_ping() + print "Erase Done." + print "Program..." + t1 = time() + self.program(filename) + print "Time: %.1fs" % (time() - t1) + print "Compare..." + t1 = time() + self.compare(filename) + print "Time: %.1fs" % (time() - t1) + print "Jump:" + self.jump(self.boot_addr) + + def write(self, s): + dev = i2c(self.i2c_addr) + dev.write(s) + dev.close() + + def read(self, num): + dev = i2c(self.i2c_addr) + s = dev.read(num) + dev.close() + return s + + def run_cmd(self, cmd, addr=0x0, num=0, data=""): + length = len(data) + s1 = struct.pack(" 1: + addr = int(args[0], 16) + if options.bToBoot: + to_bootloader(addr) + sleep(1) + if len(args) > 1: + loader = bootloader(addr) + loader.load(args[1]) diff --git a/scripts/i2c.py b/scripts/i2c.py new file mode 100755 index 0000000..2e9a109 --- /dev/null +++ b/scripts/i2c.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python +# -*- coding: iso-8859-15 -*- + +import threading +import inspect +import os +import logging +from ctypes import * +from time import sleep + +DEBUG=0 +logger = logging.getLogger(__name__) + +class i2c: + libc = CDLL("libc.so.6") + I2C_SLAVE = 0x0703 # Use this slave address + __single = None + __lock = threading.Lock() + __parent_owner = None + + def __init__(self, addr): + with i2c.__lock: + count = 0 + while(i2c.__single): + parent = inspect.stack()[1][3] + count += 1 + sleep(0.001) + if DEBUG: + if count > 10: + parent_owner = "%s (%d), %s()" % (self.__parent_owner[1], self.__parent_owner[2], self.__parent_owner[3]) + logger.warning("Error: (%s) I2C blocked %fs by %s!", parent, count*0.001, parent_owner) + i2c.__parent_owner = inspect.stack()[1] + i2c.__single = True + self.dev = i2c.libc.open("/dev/i2c-2", os.O_RDWR) + if self.dev < 0: + raise IOError("open") + err = i2c.libc.ioctl(self.dev, i2c.I2C_SLAVE, addr>>1) + if err < 0: + raise IOError("ioctl") + + def write(self, s): + num_write = i2c.libc.write(self.dev, s, len(s)) + if num_write != len(s): + self.close() + raise IOError("write: %d" % (num_write)) + + def read(self, num): + buf = create_string_buffer(num) + num_read = i2c.libc.read(self.dev, buf, num) + if num_read != num: + self.close() + raise IOError("read: %d" % (num_read)) + return buf.raw + + def close(self): + if self.dev: + i2c.libc.close(self.dev) + self.dev = None + #i2c.__parent_owner = None + i2c.__single = None + + def __del__(self): + self.close() + +if __name__ == "__main__": + import struct + import sys + + dev = i2c(0x50) + s = struct.pack(">Bh", int(sys.argv[1]), int(sys.argv[2])) + dev.write(s) + dev.close() diff --git a/scripts/move_base.py b/scripts/move_base.py new file mode 100755 index 0000000..1188e2c --- /dev/null +++ b/scripts/move_base.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python +# -*- coding: iso-8859-15 -*- + +import rospy +import struct +from i2c import i2c +from math import * +from geometry_msgs.msg import Twist + +WHEEL_DIST = 0.248 + +class MoveBase: + def __init__(self): + rospy.init_node('wild_thumper_move_base') + rospy.Subscriber("cmd_vel", Twist, self.cmdVelReceived) + self.set_speed(0, 0) + rospy.loginfo("Init done") + self.run() + + def run(self): + rate = rospy.Rate(10.0) + while not rospy.is_shutdown(): + rate.sleep() + + def set_speed(self, left, right): + if left > 255: left=255 + elif left < -255: left=-255 + if right > 255: right=255 + elif right < -255: right=-255 + + dev = i2c(0x56) + s = struct.pack(">Bhh", 0x1, left, right) + dev.write(s) + dev.close() + + def cmdVelReceived(self, msg): + trans = msg.linear.x + rot = msg.angular.z # rad/s + + right = rot*pi*WHEEL_DIST + trans + left = trans*2-right + self.set_speed(left, right) + + +if __name__ == "__main__": + MoveBase() -- 2.39.5