From a5199047dd8986cc0171f1d81f65d46555c4be41 Mon Sep 17 00:00:00 2001 From: Daniel Eisner Date: Sat, 1 Apr 2017 18:59:02 -0400 Subject: [PATCH 1/2] Move servo setup to a config file. This patch moves the controller addresses to a config file, along with the servo PWM range of motion. This allows one to use any number of servo controllers on the i2c bus, and also makes it easier to customize the servo calibration. This patch also includes a Servo Calibration GUI, which makes it much easier to calibrate both the controller and channel, as well as the min and max pwm for each servo. --- hexy.cfg | 21 +++ hexy/config.py | 49 +++++++ hexy/robot/core.py | 98 ++++++++++---- hexy_default.cfg | 21 +++ scripts/detect_controllers.py | 240 ++++++++++++++++++++++++++++++++++ 5 files changed, 402 insertions(+), 27 deletions(-) create mode 100644 hexy.cfg create mode 100644 hexy/config.py create mode 100644 hexy_default.cfg create mode 100755 scripts/detect_controllers.py diff --git a/hexy.cfg b/hexy.cfg new file mode 100644 index 0000000..233d3e1 --- /dev/null +++ b/hexy.cfg @@ -0,0 +1,21 @@ +[default] +controllers = ["0x40", "0x60"] +addr_rfh = [1, 1, 235, 470] +addr_rfk = [1, 2, 215, 522] +addr_rfa = [1, 0, 150, 750] +addr_lfh = [0, 2, 300, 580] +addr_lfk = [0, 0, 209, 450] +addr_lfa = [0, 1, 150, 750] +addr_rmh = [1, 4, 250, 600] +addr_rmk = [1, 5, 202, 476] +addr_rma = [1, 3, 183, 750] +addr_lmh = [0, 3, 176, 430] +addr_lmk = [0, 5, 176, 496] +addr_lma = [0, 4, 170, 750] +addr_rbh = [1, 6, 150, 333] +addr_rbk = [1, 8, 274, 574] +addr_rba = [1, 7, 150, 750] +addr_lbh = [0, 6, 250, 450] +addr_lbk = [0, 7, 261, 633] +addr_lba = [0, 6, 170, 750] +addr_head = [0, 15, 150, 600] diff --git a/hexy/config.py b/hexy/config.py new file mode 100644 index 0000000..8a63634 --- /dev/null +++ b/hexy/config.py @@ -0,0 +1,49 @@ +""" +This module provides a handy wrapper class for the config file. +""" + +import ConfigParser +import json +import os +import sys + +class Config(object): + """ + This class is a convenience wrapper around the hexy.cfg file. + """ + def __init__(self, filename=None): + self.filename = filename + if filename is None: + directory = os.path.dirname(__file__) + directory = os.path.dirname(directory) + self.filename = os.path.join(directory, 'hexy.cfg') + filename = self.filename + if not os.path.isfile(self.filename): + filename = os.path.join(directory, 'hexy_default.cfg') + + if not os.path.isfile(filename): + raise ValueError('%r is not a file' % filename) + + self.cfg_parser = ConfigParser.ConfigParser() + self.cfg_parser.read(filename) + + def __setitem__(self, item, value): + self.cfg_parser.set('default', item, json.dumps(value)) + + def __getitem__(self, item): + return json.loads(self.cfg_parser.get('default', item)) + + def keys(self): + return [k for k,v in self.cfg_parser.items('default')] + + def items(self): + return [(k, json.loads(v)) + for k, v in self.cfg_parser.items('default')] + + def save(self): + with open(self.filename, 'w') as outfile: + self.cfg_parser.write(outfile) + + def dumps(self): + self.cfg_parser.write(sys.stdout) + diff --git a/hexy/robot/core.py b/hexy/robot/core.py index f38fc47..ba3ea6e 100644 --- a/hexy/robot/core.py +++ b/hexy/robot/core.py @@ -1,34 +1,69 @@ from ..comm.pwm import PWM -from time import sleep +from ..config import Config +from time import sleep, time -""" joint_key convention: - R - right, L - left - F - front, M - middle, B - back - H - hip, K - knee, A - Ankle - key : (channel, minimum_pulse_length, maximum_pulse_length) """ -joint_properties = { +class Driver(object): + def __init__(self, throttle=0.0, max_load=18): + """ + Create a global driver object. + """ + self.throttle = throttle + self.max_load = max_load - 'LFH': (0, 248, 398), 'LFK': (1, 188, 476), 'LFA': (2, 131, 600), - 'RFH': (3, 275, 425), 'RFK': (4, 227, 507), 'RFA': (5, 160, 625), - 'LMH': (6, 312, 457), 'LMK': (7, 251, 531), 'LMA': (8, 138, 598), - 'RMH': (9, 240, 390), 'RMK': (10, 230, 514), 'RMA': (11, 150, 620), - 'LBH': (12, 315, 465), 'LBK': (13, 166, 466), 'LBA': (14, 140, 620), - 'RBH': (15, 320, 480), 'RBK': (16, 209, 499), 'RBA': (17, 150, 676), - 'N': (18, 150, 650) -} + self.freq = 0.50 -driver1 = PWM(0x40) -driver2 = PWM(0x41) + self.config = Config() + self.drivers = [PWM(int(addr.split('x')[-1], 16)) + for addr in self.config['controllers']] -driver1.setPWMFreq(60) -driver2.setPWMFreq(60) + for driver in self.drivers: + driver.setPWMFreq(int(self.freq*60)) + self.joint_conf = dict(self.config.items()) + self.last_cmd = {} + self.idle() -def drive(ch, val): - driver = driver1 if ch < 16 else driver2 - ch = ch if ch < 16 else ch - 16 - driver.setPWM(ch, 0, val) + def num_in_motion(self, since, excluding): + """ + return the number of servos in motion since the given time, + excluding the excluded servo. + + args: + since: time period in seconds + """ + now = time() + return len([t for t in self.last_cmd.values() + if (now - t) <= since]) + + + def drive(self, joint, val): + joint_name = 'addr_' + joint.lower() + controller, channel, pwm_min, pwm_max = self.joint_conf[joint_name] + driver = self.drivers[controller] + + while (val > 0 and + self.num_in_motion(self.throttle, joint) > self.max_load): + # Avoid putting too much load on the servos at once by + # throttling the commands we send + sleep(self.throttle / 10.0) + + driver.setPWM(channel, 0, int(self.freq*val)) + self.last_cmd[joint] = time() + + + def idle(self): + for joint in self.joint_conf: + if joint.startswith('addr_'): + self.drive(joint.split('_')[-1], 0) + + +_driver = None +def get_driver(): + global _driver + if _driver is None: + _driver = Driver() + return _driver def constrain(val, min_val, max_val): @@ -43,8 +78,16 @@ def remap(old_val, (old_min, old_max), (new_min, new_max)): class HexapodCore: def __init__(self): + """ + joint_key convention: + R - right, L - left + F - front, M - middle, B - back + H - hip, K - knee, A - Ankle + key : (controller, channel, minimum_pulse_length, maximum_pulse_length) + """ + - self.neck = Joint("neck", 'N') + self.neck = Joint("neck", 'head') self.left_front = Leg('left front', 'LFH', 'LFK', 'LFA') self.right_front = Leg('right front', 'RFH', 'RFK', 'RFA') @@ -129,7 +172,8 @@ class Joint: def __init__(self, joint_type, jkey, maxx = 90, leeway = 0): self.joint_type, self.name = joint_type, jkey - self.channel, self.min_pulse, self.max_pulse = joint_properties[jkey] + joint_addr = 'addr_' + jkey.lower() + _, _, self.min_pulse, self.max_pulse = get_driver().config[joint_addr] self.max, self.leeway = maxx, leeway self.off() @@ -139,13 +183,13 @@ def pose(self, angle = 0): angle = constrain(angle, -(self.max + self.leeway), self.max + self.leeway) pulse = remap(angle, (-self.max, self.max), (self.min_pulse, self.max_pulse)) - drive(self.channel, pulse) + get_driver().drive(self.name, pulse) self.angle = angle #print repr(self), ':', 'pulse', pulse def off(self): - drive(self.channel, 0) + get_driver().drive(self.name, 0) self.angle = None def __repr__(self): diff --git a/hexy_default.cfg b/hexy_default.cfg new file mode 100644 index 0000000..233d3e1 --- /dev/null +++ b/hexy_default.cfg @@ -0,0 +1,21 @@ +[default] +controllers = ["0x40", "0x60"] +addr_rfh = [1, 1, 235, 470] +addr_rfk = [1, 2, 215, 522] +addr_rfa = [1, 0, 150, 750] +addr_lfh = [0, 2, 300, 580] +addr_lfk = [0, 0, 209, 450] +addr_lfa = [0, 1, 150, 750] +addr_rmh = [1, 4, 250, 600] +addr_rmk = [1, 5, 202, 476] +addr_rma = [1, 3, 183, 750] +addr_lmh = [0, 3, 176, 430] +addr_lmk = [0, 5, 176, 496] +addr_lma = [0, 4, 170, 750] +addr_rbh = [1, 6, 150, 333] +addr_rbk = [1, 8, 274, 574] +addr_rba = [1, 7, 150, 750] +addr_lbh = [0, 6, 250, 450] +addr_lbk = [0, 7, 261, 633] +addr_lba = [0, 6, 170, 750] +addr_head = [0, 15, 150, 600] diff --git a/scripts/detect_controllers.py b/scripts/detect_controllers.py new file mode 100755 index 0000000..b95e2f8 --- /dev/null +++ b/scripts/detect_controllers.py @@ -0,0 +1,240 @@ +#!/usr/bin/env python +""" +Command to scan the i2c bus to locate controllers. +""" + +import sys, argparse, logging +import wx + +from hexy import config +from hexy.comm.pwm import PWM + + +class MainWindow(wx.Frame): + def __init__(self, parent, title): + width = 400 + height = 600 + + # A "-1" in the size parameter instructs wxWidgets to use the default size. + # In this case, we select 200px width and the default height. + wx.Frame.__init__(self, parent, title=title, size=(width, height)) + self.CreateStatusBar() # A Statusbar in the bottom of the window + + self.cfg = config.Config() + self.servo_list = [k for k in self.cfg.keys() if k.startswith('addr_')] + self.controller_list = self.cfg['controllers'] + self.channel_list = map(str, range(0, 15)) + + self.drivers = [PWM(int(addr.split('x')[-1], 16)) + for addr in self.controller_list] + for driver in self.drivers: + driver.setPWMFreq(60) + + ################################################################ + # Joint Selector + y_offset = 10 + self.lbl_servo_box = wx.StaticText(self, label="Joint Position", + pos=(10, y_offset)) + y_offset += 25 + self.servo_box = wx.ComboBox(self, pos=(10, y_offset), size=(120, 30), + choices=self.servo_list, + value=self.servo_list[0], + style=wx.CB_READONLY) + y_offset += 30 + self.servo_box.SetSelection(0) + self.Bind(wx.EVT_COMBOBOX, self.on_servo_select, self.servo_box) + + + ################################################################ + # Controller & Channel Selector + self.lbl_controller = wx.StaticText(self, + label="Controller", + pos=(10, y_offset)) + self.lbl_channel = wx.StaticText(self, + label="Channel", + pos=(130, y_offset)) + + y_offset += 25 + self.ctrlr_box = wx.ComboBox(self, pos=(10, y_offset), size=(120, 30), + choices=self.controller_list, + value=self.controller_list[0], + style=wx.CB_READONLY) + self.chnnl_box = wx.ComboBox(self, pos=(130, y_offset), size=(120, 30), + choices=self.channel_list, + value=self.channel_list[0], + style=wx.CB_READONLY) + self.Bind(wx.EVT_COMBOBOX, self.on_controller_select, self.ctrlr_box) + self.Bind(wx.EVT_COMBOBOX, self.on_channel_select, self.chnnl_box) + y_offset += 30 + + ################################################################ + # Pulse Slider + self.min_slider = wx.Slider(self, pos=(10, y_offset), size=(120, 40), + minValue=150, maxValue=750, + style=wx.SL_LABELS) + self.max_slider = wx.Slider(self, pos=(130, y_offset), size=(120, 40), + minValue=150, maxValue=750, + style=wx.SL_LABELS) + y_offset += 40 + self.lbl_min = wx.StaticText(self, + label="Min Pulse", + pos=(10, y_offset)) + self.lbl_max = wx.StaticText(self, + label="Max Pulse", + pos=(130, y_offset)) + y_offset += 40 + self.Bind(wx.EVT_SCROLL, self.on_min_change, self.min_slider) + self.Bind(wx.EVT_SCROLL, self.on_max_change, self.max_slider) + + ################################################################ + # Test Buttons + self.test_min = wx.Button(self, pos=(10, y_offset), size=(80, 40), + label='Test Min') + self.test_center = wx.Button(self, pos=(90, y_offset), size=(80, 40), + label='Test Center') + self.test_max = wx.Button(self, pos=(170, y_offset), size=(80, 40), + label='Test Max') + y_offset += 40 + self.Bind(wx.EVT_BUTTON, self.on_test_min, self.test_min) + self.Bind(wx.EVT_BUTTON, self.on_test_center, self.test_center) + self.Bind(wx.EVT_BUTTON, self.on_test_max, self.test_max) + + ################################################################ + # Bottom Buttons + y_offset += 20 + self.relax = wx.Button(self, pos=(10, y_offset), size=(80, 40), + label='Relax') + self.Bind(wx.EVT_BUTTON, self.on_relax, self.relax) + + self.save = wx.Button(self, pos=(100, y_offset), size=(80, 40), + label='Save') + self.Bind(wx.EVT_BUTTON, self.on_save, self.save) + + y_offset += 40 + + + # A multiline TextCtrl - This is here to show how the events + # work in this program, don't pay too much attention to it + self.logger = wx.TextCtrl(self, pos=(0,300), size=(width,300), + style=wx.TE_MULTILINE | wx.TE_READONLY) + + # Set the initial box values + self.load_settings(self.servo_list[0]) + + def load_settings(self, servo_name): + """ + Load the servo settings from the config + """ + controller, channel, min_pulse, max_pulse = self.cfg[servo_name] + + self.ctrlr_box.SetSelection(controller) + self.chnnl_box.SetSelection(channel) + self.min_slider.SetValue(min_pulse) + self.max_slider.SetValue(max_pulse) + self.logger.AppendText('Select servo: %s @ %s.%d\n' + % (servo_name, + self.controller_list[controller], + channel)) + + def save_setting(self): + """ + Saves the current GUI settings to the config (but not to disk) + """ + servo_selection = self.servo_box.GetCurrentSelection() + servo_name = self.servo_list[servo_selection] + if len(servo_name) == 0: + raise RuntimeError('No Servo Name') + self.cfg[servo_name] = self.settings_from_gui() + self.cfg.save() + + def settings_from_gui(self): + """ + Load the settings from the GUI. + """ + channel = self.chnnl_box.GetSelection() + controller = self.ctrlr_box.GetSelection() + pwm_min = self.min_slider.GetValue() + pwm_max = self.max_slider.GetValue() + return controller, channel, pwm_min, pwm_max + + def on_servo_select(self, event): + self.load_settings(event.GetString()) + + def on_controller_select(self, event): + self.logger.AppendText('Controller Select: %s\n' % event.GetString()) + + def on_channel_select(self, event): + self.logger.AppendText('Channel Select: %s\n' % event.GetString()) + + def on_min_change(self, event): + pass + + def on_max_change(self, event): + pass + + def on_test_min(self, event): + controller, channel, pwm_min, pwm_max = self.settings_from_gui() + self.logger.AppendText('Test Min: %s.%s => %s\n' + % (self.controller_list[controller], channel, pwm_min)) + driver = self.drivers[controller] + driver.setPWM(channel, 0, pwm_min) + + def on_test_center(self, event): + controller, channel, pwm_min, pwm_max = self.settings_from_gui() + pwm_center = (pwm_min + pwm_max) / 2 + self.logger.AppendText('Test Center: %s.%s => %s\n' + % (self.controller_list[controller], channel, pwm_center)) + driver = self.drivers[controller] + driver.setPWM(channel, 0, pwm_center) + + def on_test_max(self, event): + controller, channel, pwm_min, pwm_max = self.settings_from_gui() + self.logger.AppendText('Test Max: %s.%s => %s\n' + % (self.controller_list[controller], channel, pwm_max)) + driver = self.drivers[controller] + driver.setPWM(channel, 0, pwm_max) + + def on_relax(self, event): + controller, channel, pwm_min, pwm_max = self.settings_from_gui() + driver = self.drivers[controller] + driver.setPWM(channel, 0, 0) + + def on_save(self, event): + self.save_setting() + self.cfg.dumps() + +# Gather our code in a main() function +def main(args, loglevel): + logging.basicConfig(format="%(levelname)s: %(message)s", level=loglevel) + app = wx.App(False) + frame = MainWindow(None, 'Servo Tester') + frame.Show(True) + app.MainLoop() + + +# Standard boilerplate to call the main() function to begin +# the program. +if __name__ == '__main__': + parser = argparse.ArgumentParser( + description = "Does a thing to some stuff.", + epilog = "As an alternative to the commandline, params can be placed in a file, one per line, and specified on the commandline like '%(prog)s @params.conf'.", + fromfile_prefix_chars = '@' ) + # TODO Specify your real parameters here. + #parser.add_argument( + # "argument", + # help = "pass ARG to the program", + # metavar = "ARG") + parser.add_argument( + "-v", + "--verbose", + help="increase output verbosity", + action="store_true") + args = parser.parse_args() + + # Setup logging + if args.verbose: + loglevel = logging.DEBUG + else: + loglevel = logging.INFO + + main(args, loglevel) From 1a625dea148e42bf54ace1b4c61034242c737d12 Mon Sep 17 00:00:00 2001 From: Daniel Eisner Date: Sat, 8 Apr 2017 10:55:01 -0400 Subject: [PATCH 2/2] Updated docs to include new gui tool --- README.md | 39 ++++++++++++++++++++++++++++++++++++--- 1 file changed, 36 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 6b1bce8..3eca830 100644 --- a/README.md +++ b/README.md @@ -3,9 +3,8 @@ Arcbotics hexapod robot frame with Raspberry Pi Zero and Adafruit 16 channel I2C - [Hexy Documentation](http://hexyrobot.wordpress.com) - [Hexy Transcript](https://medium.com/@mithi/a-raspberry-pi-hexy-transcript-62533c69a566) -``` -HexapodCore > Hexapod > HexapodPro > DancingHexapod -``` +Quickstart +---------- The easiest way to get this up and running, on your raspberry pi zero is to do the following on the terminal via ssh. @@ -19,6 +18,11 @@ $ python -m hexy.demo.demo2 $ python -m hexy.demo.demo3 ``` +Class Hierarchy: +``` +HexapodCore > Hexapod > HexapodPro > DancingHexapod +``` + Sample usage when running python interpreter from anywhere in your system... ``` @@ -61,3 +65,32 @@ And this :) >>> hexy.lie_down() >>> hexy.curl_up(die = True) ``` + + +Configuration & Calibration +--------------------------- +You may have your servo controllers on different addresses, +or your servos plugged into different ports. You will also +have to calibrate the min and max range of each of your +servos, since these settings vary from servo to servo. + +These settings are all stored in the hexy.cfg file. To help +with this task, there is a GUI program scripts/detect_controllers.py. + +To use this program: + 1. First, edit the "controllers" line in hexy.cfg to contain + the addresses of your controllers. i2cdetect on the command + line can help you learn what to put here. See https://learn.adafruit.com/adafruit-16-channel-servo-driver-with-raspberry-pi/configuring-your-pi-for-i2c + for tips on how to do this. + 2. Next, run "python scripts/detect_controllers.py" from the top + level hexy directory. This will pop up a GUI, so you need + to have a VNC session open to your raspberry pi for this to + work. + 3. Within the gui, go one-by-one for each joint and make sure + it is assigned to the correct controller and port. You can + use "test min," "test center," "test max" both to make sure + you are using the correct controller/port, and also to + calibrate the minimum and maximum setting for each servo. + Make sure you click "save" after you are done with each joint. + This will save the settings back to the hexy.cfg file. + \ No newline at end of file