--- a/tote.hex	Mon Nov 30 19:30:45 2015 +0100
+++ b/tote.hex	Mon Nov 30 20:33:38 2015 +0100
@@ -11935,7 +11935,7 @@
 :10E9B000910D0000C91F0100C52F01003D34010069
 :04E9C000650D0000E1
 :020000040003F7
-:10E000004D508D0C696D706F7274206D6174680A6B
+:10E000004D50A50B696D706F7274206D6174680A54
 :10E01000696D706F7274207374727563740A0A6626
 :10E02000726F6D206D6963726F62697420696D70C3
 :10E030006F7274206932630A0A0A646566205F732E
@@ -12003,139 +12003,124 @@
 :10E410002F205351525432202A2073656C662E7976
 :10E420005F6469720A202020202020202073656C00
 :10E43000662E7A203D202D73656C662E74696269A4
-:10E44000610A0A20202020646566205F696E766577
-:10E450007273655F6B696E656D6174696373287350
-:10E46000656C662C20782C20792C207A293A0A2099
-:10E470002020202020202022222243616C63756C02
-:10E4800061746520746865203320616E676C657304
-:10E49000206F6620746865206C65672C20696E208B
-:10E4A000646567726565732E2222220A202020206F
-:10E4B0002020202066203D205F6E6F726D28782C12
-:10E4C000207929202D2073656C662E636F78610A90
-:10E4D000202020202020202064203D205F6E6F72AD
-:10E4E0006D28662C207A290A202020202020202038
-:10E4F00069662064203E2073656C662E66656D75C6
-:10E5000072202B2073656C662E74696269613A0A09
-:10E5100020202020202020202020202072616973CC
-:10E52000652056616C75654572726F7228224F7551
-:10E5300074206F6620726561636822290A2020209A
-:10E540002020202020686970203D206D6174682E95
-:10E550006174616E3228792C2078290A20202020CD
-:10E56000202020206B6E6565203D205F736F6C76E8
-:10E57000655F747269616E676C652873656C662E81
-:10E5800066656D75722C20642C2073656C662E7424
-:10E590006962696129202D206D6174682E61746142
-:10E5A0006E32282D7A2C2066290A20202020202057
-:10E5B0002020616E6B6C65203D205F736F6C76650B
-:10E5C0005F747269616E676C652873656C662E6630
-:10E5D000656D75722C2073656C662E746962696155
-:10E5E0002C2064290A20202020202020206B6E650A
-:10E5F00065203D206D6174682E6465677265657382
-:10E60000286B6E6565290A202020202020202061AB
-:10E610006E6B6C65203D206D6174682E6465677259
-:10E6200065657328616E6B6C6529202D2039300A71
-:10E630002020202020202020686970203D202D6D82
-:10E640006174682E646567726565732868697029EE
-:10E65000202D203435202A2073656C662E795F6466
-:10E6600069720A2020202020202020726574757293
-:10E670006E206869702C206B6E65652C20616E6B56
-:10E680006C650A0A20202020646566206D6F76651F
-:10E690005F746F2873656C662C20783D4E6F6E65D5
-:10E6A0002C20793D4E6F6E652C207A3D4E6F6E6545
-:10E6B000293A0A20202020202020202222224D6FCB
-:10E6C00076652074686520666F6F7420746F20749F
-:10E6D00068652073706563696669656420706F6939
-:10E6E0006E742E0A0A202020202020202049662037
-:10E6F000616E79206F662074686520636F6F726445
-:10E70000696E61746573206973204E6F6E652C208D
-:10E710006E6F206D6F76656D656E742077696C6CB9
-:10E7200020626520646F6E6520696E207468617474
-:10E730000A2020202020202020617869732E0A20C2
-:10E74000202020202020202222220A2020202020D9
-:10E7500020202069662078206973204E6F6E653A0C
-:10E760000A20202020202020202020202078203D4A
-:10E770002073656C662E780A20202020202020201F
-:10E7800069662079206973204E6F6E653A0A2020F1
-:10E790002020202020202020202079203D2073656B
-:10E7A0006C662E790A2020202020202020696620F7
-:10E7B0007A206973204E6F6E653A0A20202020204F
-:10E7C000202020202020207A203D2073656C662E9A
-:10E7D0007A0A202020202020202073656C662E7568
-:10E7E000706461746528782C20792C207A290A209D
-:10E7F0002020202020202073656C662E78203D206C
-:10E80000780A202020202020202073656C662E7935
-:10E81000203D20790A202020202020202073656CB4
-:10E82000662E7A203D207A0A0A2020202064656620
-:10E83000206D6F76655F62792873656C662C206445
-:10E84000783D302C2064793D302C20647A3D30298D
-:10E850003A0A20202020202020202222224D6F76DC
-:10E86000652074686520666F6F7420627920746813
-:10E87000652073706563696669656420616D6F7595
-:10E880006E742E2222220A20202020202020207395
-:10E89000656C662E6D6F76655F746F2873656C6648
-:10E8A0002E78202B2064782C2073656C662E7920BE
-:10E8B0002B2064792C2073656C662E7A202B2064C3
-:10E8C0007A290A0A2020202064656620726F74610C
-:10E8D00074655F62792873656C662C206465677265
-:10E8E000656573293A0A202020202020202022223A
-:10E8F00022526F7461746520746865206C656720AE
-:10E9000061726F756E64207468652063656E7465EE
-:10E9100072206F662074686520626F647920686F6A
-:10E9200072697A6F6E74616C6C792E2222220A20D1
-:10E930002020202020202072616469616E73203DB8
-:10E94000206D6174682E72616469616E73286465FC
-:10E950006772656573290A202020202020202078F6
-:10E96000203D2073656C662E78202B2073656C66C5
-:10E970002E626173650A202020202020202079202B
-:10E980003D2073656C662E79202B2073656C662E96
-:10E99000626173650A20202020202020206E7820CC
-:10E9A0003D2078202A206D6174682E636F73287271
-:10E9B000616469616E7329202D2079202A206D61A0
-:10E9C00074682E73696E2872616469616E732920A0
-:10E9D0002D2073656C662E626173650A20202020ED
-:10E9E000202020206E79203D2078202A206D61741F
-:10E9F000682E73696E2872616469616E7329202BB9
-:10EA00002079202A206D6174682E636F73287261EB
-:10EA10006469616E7329202D2073656C662E6261B6
-:10EA200073650A202020202020202073656C662E2C
-:10EA30006D6F76655F746F286E782C206E79290A69
-:10EA40000A20202020646566207570646174652842
-:10EA500073656C662C20782C20792C207A293A0A50
-:10EA6000202020202020202022222253656E642096
-:10EA700074686520706F736974696F6E7320746F4A
-:10EA80002074686520736572766F732E2222220AC5
-:10EA90002020202020202020616E676C6573203D9F
-:10EAA0002073656C662E5F696E76657273655F6B49
-:10EAB000696E656D617469637328782C20792C20E8
-:10EAC0007A290A2020202020202020666F722073BF
-:10EAD0006572766F2C20616E676C6520696E207A96
-:10EAE00069702873656C662E736572766F732C205F
-:10EAF000616E676C6573293A0A202020202020204F
-:10EB000020202020206932632E77726974652830B6
-:10EB1000783034203C3C20312C0A2020202020203A
-:10EB200020202020202020202020202020202020E5
-:10EB30007374727563742E7061636B28273C424254
-:10EB400066272C20736572766F2C20302C206D6127
-:10EB500074682E72616469616E7328616E676C659A
-:10EB60002929290A0A0A636C61737320526F626F44
-:10EB7000743A0A20202020646566205F5F696E6910
-:10EB8000745F5F2873656C66293A0A202020202074
-:10EB900020202073656C662E6C656773203D205BBA
-:10EBA0000A2020202020202020202020204C6567C3
-:10EBB00028322C20312C20302C202D312C203129B2
-:10EBC0002C0A2020202020202020202020204C65DE
-:10EBD0006728352C20342C20332C202D312C202D4F
-:10EBE00031292C0A20202020202020202020202015
-:10EBF0004C656728382C20372C20362C20312C20CF
-:10EC000031292C0A202020202020202020202020F4
-:10EC10004C65672831312C2031302C20392C2031A3
-:10EC20002C202D31292C0A20202020202020205D7E
-:10EC30000A0A20202020646566207570646174656E
-:10EC40002873656C66293A0A202020202020202085
-:10EC5000666F72206C656720696E2073656C662E26
-:10EC60006C6567733A0A2020202020202020202075
-:10EC700020206C65672E75706461746528290A0A06
-:10EC80000A726F626F74203D20526F626F74282980
-:10EC90000A0000000000000000000000000000006A
+:10E44000610A0A20202020646566206D6F76655F72
+:10E45000746F2873656C662C20783D4E6F6E652C4A
+:10E4600020793D4E6F6E652C207A3D4E6F6E65298A
+:10E470003A0A20202020202020202222224D6F76C0
+:10E48000652074686520666F6F7420746F207468EF
+:10E49000652073706563696669656420706F696E75
+:10E4A000742E0A0A20202020202020204966206186
+:10E4B0006E79206F662074686520636F6F7264697F
+:10E4C0006E61746573206973204E6F6E652C206ECB
+:10E4D0006F206D6F76656D656E742077696C6C204A
+:10E4E000626520646F6E6520696E20746861740ACD
+:10E4F0002020202020202020617869732E0A2020EF
+:10E500002020202020202222220A2020202020201B
+:10E51000202069662078206973204E6F6E653A0A64
+:10E5200020202020202020202020202078203D2076
+:10E5300073656C662E780A20202020202020206918
+:10E54000662079206973204E6F6E653A0A2020207C
+:10E5500020202020202020202079203D2073656C61
+:10E56000662E790A20202020202020206966207A2B
+:10E57000206973204E6F6E653A0A202020202020EB
+:10E580002020202020207A203D2073656C662E7A82
+:10E590000A202020202020202066203D205F6E6F52
+:10E5A000726D28782C207929202D2073656C662EB9
+:10E5B000636F78610A202020202020202064203DE5
+:10E5C000205F6E6F726D28662C207A290A20202029
+:10E5D000202020202069662064203E2073656C6620
+:10E5E0002E66656D7572202B2073656C662E7469BE
+:10E5F0006269613A0A20202020202020202020204B
+:10E600002072616973652056616C75654572726F21
+:10E610007228224F7574206F6620726561636822CC
+:10E62000290A2020202020202020686970203D20F9
+:10E630006D6174682E6174616E3228792C2078299E
+:10E640000A20202020202020206B6E6565203D20A0
+:10E650005F736F6C76655F747269616E676C652855
+:10E6600073656C662E66656D75722C20642C207344
+:10E67000656C662E746962696129202D206D617454
+:10E68000682E6174616E32282D7A2C2066290A204A
+:10E6900020202020202020616E6B6C65203D205FB3
+:10E6A000736F6C76655F747269616E676C652873F1
+:10E6B000656C662E66656D75722C2073656C662EB2
+:10E6C00074696269612C2064290A2020202020209E
+:10E6D00020206B6E6565203D206D6174682E646539
+:10E6E0006772656573286B6E6565290A2020202096
+:10E6F00020202020616E6B6C65203D206D61746868
+:10E700002E6465677265657328616E6B6C65292080
+:10E710002D2039300A2020202020202020686970F8
+:10E72000203D202D6D6174682E6465677265657388
+:10E730002868697029202D203435202A2073656CC3
+:10E74000662E795F6469720A202020202020202014
+:10E75000666F7220736572766F2C20616E676C65D0
+:10E7600020696E207A69702873656C662E736572F5
+:10E77000766F732C20286869702C206B6E65652C71
+:10E7800020616E6B6C6529293A0A20202020202008
+:10E790002020202020206932632E7772697465283A
+:10E7A00030783034203C3C20312C0A20202020209E
+:10E7B0002020202020202020202020202020202059
+:10E7C000207374727563742E7061636B28273C42EA
+:10E7D0004266272C20736572766F2C20302C206DBA
+:10E7E0006174682E72616469616E7328616E676C12
+:10E7F000652929290A202020202020202073656CEB
+:10E80000662E78203D20780A2020202020202020FD
+:10E8100073656C662E79203D20790A202020202007
+:10E8200020202073656C662E7A203D207A0A0A200B
+:10E83000202020646566206D6F76655F627928739D
+:10E84000656C662C2064783D302C2064793D302C3A
+:10E8500020647A3D30293A0A2020202020202020E0
+:10E860002222224D6F76652074686520666F6F7472
+:10E8700020627920746865207370656369666965D4
+:10E880006420616D6F756E742E2222220A20202072
+:10E89000202020202073656C662E6D6F76655F7476
+:10E8A0006F2873656C662E78202B2064782C20737B
+:10E8B000656C662E79202B2064792C2073656C663C
+:10E8C0002E7A202B20647A290A0A202020206465D1
+:10E8D0006620726F746174655F62792873656C6617
+:10E8E0002C2064656772656573293A0A2020202010
+:10E8F00020202020222222526F74617465207468C7
+:10E9000065206C65672061726F756E642074686540
+:10E910002063656E746572206F662074686520627E
+:10E920006F647920686F72697A6F6E74616C6C794C
+:10E930002E2222220A202020202020202072616402
+:10E9400069616E73203D206D6174682E7261646927
+:10E95000616E732864656772656573290A202020DB
+:10E96000202020202078203D2073656C662E7820A2
+:10E970002B2073656C662E626173650A202020204F
+:10E980002020202079203D2073656C662E79202B75
+:10E990002073656C662E626173650A20202020203A
+:10E9A0002020206E78203D2078202A206D61746818
+:10E9B0002E636F732872616469616E7329202D2044
+:10E9C00079202A206D6174682E73696E28726164E3
+:10E9D00069616E7329202D2073656C662E626173E8
+:10E9E000650A20202020202020206E79203D2078DC
+:10E9F000202A206D6174682E73696E2872616469C3
+:10EA0000616E7329202B2079202A206D6174682E75
+:10EA1000636F732872616469616E7329202D20739E
+:10EA2000656C662E626173650A20202020202020FC
+:10EA30002073656C662E6D6F76655F746F286E78D7
+:10EA40002C206E79290A0A0A636C61737320526F55
+:10EA5000626F743A0A20202020646566205F5F6937
+:10EA60006E69745F5F2873656C66293A0A202020FE
+:10EA7000202020202073656C662E6C656773203D16
+:10EA8000205B0A2020202020202020202020204C35
+:10EA9000656728322C20312C20302C20312C202D61
+:10EAA00031292C0A20202020202020202020202056
+:10EAB0004C656728352C20342C20332C202D312C0C
+:10EAC000202D31292C0A2020202020202020202029
+:10EAD00020204C656728382C20372C20362C202D00
+:10EAE000312C2031292C0A202020202020202020F9
+:10EAF0002020204C65672831312C2031302C2039E2
+:10EB00002C20312C2031292C0A20202020202020CC
+:10EB1000205D0A0A20202020646566206D6F7665DE
+:10EB20005F62792873656C662C2064783D302C20F8
+:10EB300064793D302C20647A3D30293A0A20202027
+:10EB40002020202020666F72206C656720696E206F
+:10EB500073656C662E6C6567733A0A20202020204E
+:10EB6000202020202020206C65672E6D6F76655F49
+:10EB70006279286478202A206C65672E785F646942
+:10EB8000722C206479202A206C65672E795F646975
+:10EB9000722C20647A290A0A0A726F626F74203D0F
+:10EBA00020526F626F7428290A00000000000000E4
 :0400000500011C6D6D
 :00000001FF

--- a/tote.py	Mon Nov 30 19:30:45 2015 +0100
+++ b/tote.py	Mon Nov 30 20:33:38 2015 +0100
@@ -34,20 +34,6 @@
         self.y = -(self.coxa + self.femur) / SQRT2 * self.y_dir
         self.z = -self.tibia
 
-    def _inverse_kinematics(self, x, y, z):
-        """Calculate the 3 angles of the leg, in degrees."""
-        f = _norm(x, y) - self.coxa
-        d = _norm(f, z)
-        if d > self.femur + self.tibia:
-            raise ValueError("Out of reach")
-        hip = math.atan2(y, x)
-        knee = _solve_triangle(self.femur, d, self.tibia) - math.atan2(-z, f)
-        ankle = _solve_triangle(self.femur, self.tibia, d)
-        knee = math.degrees(knee)
-        ankle = math.degrees(ankle) - 90
-        hip = -math.degrees(hip) - 45 * self.y_dir
-        return hip, knee, ankle
-
     def move_to(self, x=None, y=None, z=None):
         """Move the foot to the specified point.
 
@@ -60,7 +46,19 @@
             y = self.y
         if z is None:
             z = self.z
-        self.update(x, y, z)
+        f = _norm(x, y) - self.coxa
+        d = _norm(f, z)
+        if d > self.femur + self.tibia:
+            raise ValueError("Out of reach")
+        hip = math.atan2(y, x)
+        knee = _solve_triangle(self.femur, d, self.tibia) - math.atan2(-z, f)
+        ankle = _solve_triangle(self.femur, self.tibia, d)
+        knee = math.degrees(knee)
+        ankle = math.degrees(ankle) - 90
+        hip = -math.degrees(hip) - 45 * self.y_dir
+        for servo, angle in zip(self.servos, (hip, knee, ankle)):
+            i2c.write(0x04 << 1,
+                      struct.pack('<BBf', servo, 0, math.radians(angle)))
         self.x = x
         self.y = y
         self.z = z
@@ -78,26 +76,19 @@
         ny = x * math.sin(radians) + y * math.cos(radians) - self.base
         self.move_to(nx, ny)
 
-    def update(self, x, y, z):
-        """Send the positions to the servos."""
-        angles = self._inverse_kinematics(x, y, z)
-        for servo, angle in zip(self.servos, angles):
-            i2c.write(0x04 << 1,
-                      struct.pack('<BBf', servo, 0, math.radians(angle)))
-
 
 class Robot:
     def __init__(self):
         self.legs = [
-            Leg(2, 1, 0, -1, 1),
+            Leg(2, 1, 0, 1, -1),
             Leg(5, 4, 3, -1, -1),
-            Leg(8, 7, 6, 1, 1),
-            Leg(11, 10, 9, 1, -1),
+            Leg(8, 7, 6, -1, 1),
+            Leg(11, 10, 9, 1, 1),
         ]
 
-    def update(self):
+    def move_by(self, dx=0, dy=0, dz=0):
         for leg in self.legs:
-            leg.update()
+            leg.move_by(dx * leg.x_dir, dy * leg.y_dir, dz)
 
 
 robot = Robot()