Mercurial > pony
changeset 6:1529d6a90448 default tip
Robot body code
| author | Radomir Dopieralski <sheep-devel@sheep.art.pl> |
|---|---|
| date | Sat, 10 Sep 2016 14:25:40 +0200 |
| parents | c1b6dd72fb3c |
| children | |
| files | boot.py leg.py machine.py robot.py |
| diffstat | 4 files changed, 63 insertions(+), 7 deletions(-) [+] |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/boot.py Sat Sep 10 14:25:40 2016 +0200 @@ -0,0 +1,8 @@ +# This file is executed on every boot (including wake-boot from deepsleep) +import esp +esp.osdebug(None) +import gc +import webrepl +webrepl.start() +gc.collect() +
--- a/leg.py Tue May 10 17:56:10 2016 +0200 +++ b/leg.py Sat Sep 10 14:25:40 2016 +0200 @@ -1,7 +1,7 @@ import math -from machine import Pin + -from servo import Servo +_pi2 = math.pi / 2 def _solve_triangle(a, b, c): @@ -18,17 +18,25 @@ class Leg: femur = 21 # mm tibia = 27 # mm + hip_trim = -_pi2 + knee_trim = _pi2 - def __init__(self, hip_pin, knee_pin): - self.servos = (Servo(Pin(hip_pin)), Servo(Pin(knee_pin))) + def __init__(self, hip_servo, knee_servo): + self.servos = hip_servo, knee_servo def inverse_kinematics(self, x, y): d = _norm(x, y) if d > self.femur + self.tibia: raise ValueError("Out of range.") - hip = math.atan2(y, x) + _solve_triangle(self.femur, d, self.tibia) - knee = _solve_triangle(self.femur, self.tibia, d) - return hip, knee + hip = math.atan2(x, y) + _solve_triangle(self.femur, d, self.tibia) + knee = _solve_triangle(self.femur, self.tibia, d) - math.pi + return hip + self.hip_trim, knee + self.knee_trim + + def move_to(self, x, y): + ik = self.inverse_kinematics(x, y - self.femur - self.tibia) + for servo, angle in zip(self.servos, ik): + print(math.degrees(angle)) + servo.write_angle(radians=angle) class HindLeg(Leg):
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/machine.py Sat Sep 10 14:25:40 2016 +0200 @@ -0,0 +1,14 @@ + +class Pin: + def __init__(self, index): + self.index = index + +class PWM: + def __init__(self, pin, freq, duty): + self.pin = pin + self.freq = freq + self._duty = duty + + def duty(self, duty): + self._duty = duty + print("Pin %d duty %d" % (self.pin.index, self._duty))
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/robot.py Sat Sep 10 14:25:40 2016 +0200 @@ -0,0 +1,26 @@ +from machine import Pin +import time + +from servo import Servo +from leg import Leg, HindLeg + +""" +import robot +r = robot.Robot() +l = r.legs[0] +l.move_to(0, 0) +""" +class Robot: + def __init__(self): + self.legs = ( + Leg(Servo(Pin(15)), Servo(Pin(2))), # front left + Leg(Servo(Pin(14)), Servo(Pin(0))), # front right + HindLeg(Servo(Pin(13)), Servo(Pin(12))), # hind right + HindLeg(Servo(Pin(4)), Servo(Pin(5))), # hind left + ) + + def home(self): + for leg in self.legs: + for servo in leg.servos: + servo.write_us(1500) + time.sleep_ms(200)