--- a/tote.hex	Mon Nov 30 20:33:38 2015 +0100
+++ b/tote.hex	Mon Nov 30 23:48:59 2015 +0100
@@ -11935,7 +11935,7 @@
 :10E9B000910D0000C91F0100C52F01003D34010069
 :04E9C000650D0000E1
 :020000040003F7
-:10E000004D50A50B696D706F7274206D6174680A54
+:10E000004D509C0B696D706F7274206D6174680A5D
 :10E01000696D706F7274207374727563740A0A6626
 :10E02000726F6D206D6963726F62697420696D70C3
 :10E030006F7274206932630A0A0A646566205F732E
@@ -11969,158 +11969,158 @@
 :10E1F0006C617373204C65673A0A2020202023202D
 :10E200004C656E67746873206F66206C6567207359
 :10E2100065676D656E74732C20696E206D6D0A20C4
-:10E22000202020636F7861203D2032322E3520205F
-:10E2300023206265747765656E207468652068695F
-:10E240007020616E6420746865206B6E65650A20BD
-:10E2500020202066656D7572203D203338202023F4
-:10E26000206265747765656E20746865206B6E65E5
-:10E270006520616E642074686520616E6B6C650A50
-:10E28000202020207469626961203D2033302020E5
-:10E2900023206265747765656E2074686520616E01
-:10E2A0006B6C6520616E642074686520666F6F74A6
-:10E2B0000A2020202062617365203D203135202016
-:10E2C000232066726F6D207468652063656E7465C7
-:10E2D00072206F6620626F64790A0A202020206411
-:10E2E0006566205F5F696E69745F5F2873656C6641
-:10E2F0002C206869705F736572766F2C206B6E6579
-:10E30000655F736572766F2C20616E6B6C655F73F1
-:10E310006572766F2C20785F6469722C20795F6457
-:10E320006972293A0A202020202020202073656C61
-:10E33000662E736572766F73203D20286869705F62
-:10E34000736572766F2C206B6E65655F7365727690
-:10E350006F2C20616E6B6C655F736572766F290A36
-:10E36000202020202020202073656C662E785F649A
-:10E370006972203D20785F6469720A202020202085
-:10E3800020202073656C662E795F646972203D20C1
-:10E39000795F6469720A20202020202020205351B8
-:10E3A000525432203D206D6174682E73717274284E
-:10E3B00032290A202020202020202073656C662E20
-:10E3C00078203D202873656C662E636F7861202B62
-:10E3D0002073656C662E66656D757229202F20533B
-:10E3E000515254320A202020202020202073656CB6
-:10E3F000662E79203D202D2873656C662E636F781C
-:10E4000061202B2073656C662E66656D7572292000
-:10E410002F205351525432202A2073656C662E7976
-:10E420005F6469720A202020202020202073656C00
-:10E43000662E7A203D202D73656C662E74696269A4
-: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
+:10E22000202020636F7861203D2020313220202083
+:10E230002023206265747765656E207468652068A8
+:10E24000697020616E6420746865206B6E65650A74
+:10E250002020202066656D7572203D2034322020FC
+:10E26000202023206265747765656E2074686520C0
+:10E270006B6E656520616E642074686520616E6BED
+:10E280006C650A202020207469626961203D20337A
+:10E29000382E35202023206265747765656E2074E2
+:10E2A000686520616E6B6C6520616E6420746865C2
+:10E2B00020666F6F740A2020202062617365203D04
+:10E2C0002020323120202020232066726F6D2074A0
+:10E2D00068652063656E746572206F6620626F6486
+:10E2E000790A0A20202020646566205F5F696E69D4
+:10E2F000745F5F2873656C662C206869705F736556
+:10E3000072766F2C206B6E65655F736572766F2C0D
+:10E3100020616E6B6C655F736572766F2C20785F21
+:10E320006469722C20795F646972293A0A2020207E
+:10E33000202020202073656C662E736572766F73C3
+:10E34000203D20286869705F736572766F2C206BA2
+:10E350006E65655F736572766F2C20616E6B6C65A0
+:10E360005F736572766F290A2020202020202020EC
+:10E3700073656C662E785F646972203D20785F64F7
+:10E3800069720A202020202020202073656C662ED0
+:10E39000795F646972203D20795F6469720A202088
+:10E3A0002020202020205351525432203D206D61E6
+:10E3B00074682E737172742832290A20202020205C
+:10E3C00020202073656C662E78203D202873656CB4
+:10E3D000662E636F7861202B2073656C662E6665F0
+:10E3E0006D757229202F2053515254320A2020205B
+:10E3F000202020202073656C662E79203D20287314
+:10E40000656C662E636F7861202B2073656C662EB9
+:10E4100066656D757229202F2053515254320A209F
+:10E420002020202020202073656C662E7A203D203D
+:10E430002D73656C662E74696269610A0A2020205A
+:10E4400020646566206D6F76655F746F2873656CF8
+:10E45000662C20783D4E6F6E652C20793D4E6F6E98
+:10E46000652C207A3D4E6F6E65293A0A20202020C7
+:10E47000202020202222224D6F766520746865209E
+:10E48000666F6F7420746F207468652073706563A5
+:10E49000696669656420706F696E742E0A0A2020AF
+:10E4A000202020202020496620616E79206F662080
+:10E4B00074686520636F6F7264696E617465732040
+:10E4C0006973204E6F6E652C206E6F206D6F7665C0
+:10E4D0006D656E742077696C6C20626520646F6E68
+:10E4E0006520696E20746861740A20202020202035
+:10E4F0002020617869732E0A2020202020202020EF
+:10E500002222220A20202020202020206966207834
+:10E51000206973204E6F6E653A0A2020202020204B
+:10E5200020202020202078203D2073656C662E78E6
+:10E530000A2020202020202020696620792069736D
+:10E54000204E6F6E653A0A202020202020202020B7
+:10E5500020202079203D2073656C662E790A2020CA
+:10E560002020202020206966207A206973204E6FA9
+:10E570006E653A0A20202020202020202020202004
+:10E580007A203D2073656C662E7A0A202020202098
+:10E5900020202066203D205F6E6F726D28782C2031
+:10E5A0007929202D2073656C662E636F78610A20AF
+:10E5B0002020202020202064203D205F6E6F726D7F
+:10E5C00028662C207A290A2020202020202020695B
+:10E5D000662064203E2073656C662E66656D7572DC
+:10E5E000202B2073656C662E74696269613A0A207B
+:10E5F00020202020202020202020207261697365A7
+:10E600002056616C75654572726F7228224F757461
+:10E61000206F6620726561636822290A202020200D
+:10E6200020202020686970203D206D6174682E6173
+:10E6300074616E3228792C2078290A20202020202D
+:10E640002020206B6E6565203D205F736F6C7665C2
+:10E650005F747269616E676C652873656C662E669F
+:10E66000656D75722C20642C2073656C662E746940
+:10E6700062696129202D206D6174682E6174616E5C
+:10E6800032282D7A2C2066290A20202020202020C4
+:10E6900020616E6B6C65203D205F736F6C76655FEB
+:10E6A000747269616E676C652873656C662E666549
+:10E6B0006D75722C2073656C662E74696269612CAD
+:10E6C0002064290A0A202020202020202068697048
+:10E6D000203D206D6174682E6465677265657328DE
+:10E6E00068697029202D2034350A20202020202020
+:10E6F00020206B6E6565203D206D6174682E646519
+:10E700006772656573286B6E6565290A2020202075
+:10E7100020202020616E6B6C65203D206D61746847
+:10E720002E6465677265657328616E6B6C65292060
+:10E730002D2039300A2020202020202020666F72D2
+:10E7400020736572766F2C20616E676C6520696E30
+:10E75000207A69702873656C662E736572766F73A4
+:10E760002C20286869702C206B6E65652C20616EEA
+:10E770006B6C6529293A0A202020202020202020A7
+:10E780002020206932632E777269746528307830D2
+:10E7900034203C3C20312C0A202020202020202026
+:10E7A00020202020202020202020202020207374C2
+:10E7B000727563742E7061636B28273C4242662732
+:10E7C0002C20736572766F2C20302C206D6174685C
+:10E7D0002E72616469616E7328616E676C652929A8
+:10E7E000290A202020202020202073656C662E78A6
+:10E7F000203D20780A202020202020202073656CD6
+:10E80000662E79203D20790A2020202020202020FB
+:10E8100073656C662E7A203D207A0A0A202020201B
+:10E82000646566206D6F76655F62792873656C66D6
+:10E830002C2064783D302C2064793D302C20647A83
+:10E840003D30293A0A202020202020202022222288
+:10E850004D6F76652074686520666F6F74206279ED
+:10E8600020746865207370656369666965642061FA
+:10E870006D6F756E742E2222220A20202020202007
+:10E88000202073656C662E6D6F76655F746F2873DC
+:10E89000656C662E78202B2064782C2073656C665E
+:10E8A0002E79202B2064792C2073656C662E7A20BB
+:10E8B0002B20647A290A0A202020206465662072B1
+:10E8C0006F746174655F62792873656C662C20646F
+:10E8D000656772656573293A0A2020202020202070
+:10E8E00020222222526F7461746520746865206C46
+:10E8F00065672061726F756E64207468652063655A
+:10E900006E746572206F662074686520626F64792A
+:10E9100020686F72697A6F6E74616C6C792E222236
+:10E92000220A202020202020202072616469616E4C
+:10E9300073203D206D6174682E72616469616E732D
+:10E940002864656772656573290A202020202020CD
+:10E95000202078203D2073656C662E78202B207354
+:10E96000656C662E626173650A20202020202020BD
+:10E970002079203D2073656C662E79202B207365ED
+:10E980006C662E626173650A2020202020202020E2
+:10E990006E78203D2078202A206D6174682E636F88
+:10E9A000732872616469616E7329202D2079202A91
+:10E9B000206D6174682E73696E2872616469616E7E
+:10E9C0007329202D2073656C662E626173650A20A1
+:10E9D000202020202020206E79203D2078202A2011
+:10E9E0006D6174682E73696E2872616469616E73FB
+:10E9F00029202B2079202A206D6174682E636F7383
+:10EA00002872616469616E7329202D2073656C66BC
+:10EA10002E626173650A202020202020202073654B
+:10EA20006C662E6D6F76655F746F286E782C206E25
+:10EA300079290A0A0A636C61737320526F626F74DA
+:10EA40003A0A20202020646566205F5F696E697441
+:10EA50005F5F2873656C66293A0A202020202020F9
+:10EA6000202073656C662E6C656773203D205B0A01
+:10EA70002020202020202020202020204C656728D6
+:10EA800020322C2020312C20302C202D312C202005
+:10EA900031292C0A20202020202020202020202066
+:10EAA0004C65672820352C2020342C20332C202D39
+:10EAB000312C202D31292C0A20202020202020201C
+:10EAC000202020204C65672820382C2020372C203F
+:10EAD000362C2020312C202D31292C0A20202020DA
+:10EAE00020202020202020204C65672831312C2038
+:10EAF00031302C20392C2020312C202031292C0A97
+:10EB000020202020202020205D0A0A2020202064B0
+:10EB10006566206D6F76655F62792873656C662C1B
+:10EB20002064783D302C2064793D302C20647A3D7F
+:10EB300030293A0A2020202020202020666F7220D1
+:10EB40006C656720696E2073656C662E6C656773F3
+:10EB50003A0A2020202020202020202020206C6520
+:10EB6000672E6D6F76655F6279286478202A206C45
+:10EB700065672E785F6469722C206479202A206C86
+:10EB800065672E795F6469722C20647A290A0A0A03
+:10EB9000726F626F74203D20526F626F7428290A71
+:10EBA0000000000000000000000000000000000065
 :0400000500011C6D6D
 :00000001FF

--- a/tote.py	Mon Nov 30 20:33:38 2015 +0100
+++ b/tote.py	Mon Nov 30 23:48:59 2015 +0100
@@ -20,10 +20,10 @@
 
 class Leg:
     # Lengths of leg segments, in mm
-    coxa = 22.5  # between the hip and the knee
-    femur = 38  # between the knee and the ankle
-    tibia = 30  # between the ankle and the foot
-    base = 15  # from the center of body
+    coxa =  12    # between the hip and the knee
+    femur = 42    # between the knee and the ankle
+    tibia = 38.5  # between the ankle and the foot
+    base =  21    # from the center of body
 
     def __init__(self, hip_servo, knee_servo, ankle_servo, x_dir, y_dir):
         self.servos = (hip_servo, knee_servo, ankle_servo)
@@ -31,7 +31,7 @@
         self.y_dir = y_dir
         SQRT2 = math.sqrt(2)
         self.x = (self.coxa + self.femur) / SQRT2
-        self.y = -(self.coxa + self.femur) / SQRT2 * self.y_dir
+        self.y = (self.coxa + self.femur) / SQRT2
         self.z = -self.tibia
 
     def move_to(self, x=None, y=None, z=None):
@@ -53,9 +53,10 @@
         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)
+
+        hip = math.degrees(hip) - 45
         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)))
@@ -80,10 +81,10 @@
 class Robot:
     def __init__(self):
         self.legs = [
-            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( 2,  1, 0, -1,  1),
+            Leg( 5,  4, 3, -1, -1),
+            Leg( 8,  7, 6,  1, -1),
+            Leg(11, 10, 9,  1,  1),
         ]
 
     def move_by(self, dx=0, dy=0, dz=0):