Move servo setup to a config file.

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.
+

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]

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)
+

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):

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]