Mercurial > pyb_tote
changeset 0:99a77cc40c20 default tip
Initial commit
| author | Radomir Dopieralski <sheep-devel@sheep.art.pl> |
|---|---|
| date | Sat, 19 Dec 2015 22:02:01 +0100 |
| parents | |
| children | |
| files | .hgignore gait.py main.py robot.py servo.py |
| diffstat | 5 files changed, 218 insertions(+), 0 deletions(-) [+] |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/.hgignore Sat Dec 19 22:02:01 2015 +0100 @@ -0,0 +1,5 @@ +syntax: glob +*~ +.*.swp +*.DS_Store +doc/_build
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/gait.py Sat Dec 19 22:02:01 2015 +0100 @@ -0,0 +1,74 @@ +# -*- coding: utf-8 -*- + +import pyb + + +class Creep: + speed = 0.75 + stride = 14 + shift = 8 + shift_steps = 2 + raise_leg = 10 + raise_steps = 2 + next_leg = ( + (2, 0, 3, 1), # forward + (1, 3, 0, 2), # backward + (1, 2, 3, 0), # clockwise + (3, 0, 1, 2), # counter-clockwise + (2, 3, 1, 0), # left + (3, 2, 0, 1), # right + ) + + def __init__(self, robot): + self.robot = robot + self.shift_x = 0 + self.shift_y = 0 + self.on_ground = list(robot.legs) + self.dx = 0 + self.dy = 1 + self.rotation = 0 + self.spread = 0 + leg = robot.legs[0] + self.height = leg.tibia + self.last_leg = 0 + + def move(self): + for leg in self.on_ground: + dx = self.dx * leg.x_dir * self.speed + dy = self.dy * leg.y_dir * self.speed + leg.move_to(leg.x - dx, leg.y - dy, -self.height) + #leg.rotate_by(-self.rotation * leg.x_dir * leg.y_dir / 2) + pyb.delay(25) + + def step(self, leg): + dx = (leg.x_dir * self.shift - self.shift_x) / self.shift_steps + dy = (leg.y_dir * self.shift - self.shift_y) / self.shift_steps + for step in range(self.shift_steps): + self.robot.move_by(dx, dy, 0) + self.shift_x += dx + self.shift_y += dy + self.move() + self.on_ground.remove(leg) + for step in range(self.raise_steps): + leg.move_by(0, 0, self.raise_leg) + self.move() + leg.move_to( + leg.home + (self.shift_x + self.dx * + self.stride) * leg.x_dir + self.spread, + leg.home + (self.shift_y + self.dy * + self.stride) * leg.y_dir + self.spread, + self.raise_leg * self.raise_steps - self.height + ) + self.move() + self.move() + for step in range(self.raise_steps - 1): + leg.move_by(0, 0, -self.raise_leg) + self.move() + self.on_ground.append(leg) + self.move() + + def run(self, steps): + for step in range(steps): + leg = self.robot.legs[self.last_leg] + self.step(leg) + self.last_leg = self.next_leg[0][self.last_leg]
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.py Sat Dec 19 22:02:01 2015 +0100 @@ -0,0 +1,9 @@ +# -*- coding: utf-8 -*- + +from robot import Robot +from gait import Creep + + +robot = Robot() +gait = Creep(robot) +#gait.run(16)
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/robot.py Sat Dec 19 22:02:01 2015 +0100 @@ -0,0 +1,80 @@ +# -*- coding: utf-8 -*- + +import math + +from servo import Servo + + +PI2 = math.pi / 2 +PI4 = math.pi / 4 + + +def _solve_triangle(a, b, c): + a, b, c = abs(a), abs(b), abs(c) + if a + b < c or a + c < b or b + c < a: + raise ValueError("Impossible triangle: %r, %r, %r" % (a, b, c)) + cos = float(a ** 2 + b ** 2 - c ** 2) / (2 * a * b) + return math.acos(cos) + + +def _norm(a, b): + return math.sqrt(a ** 2 + b ** 2) + + +class Leg: + # Lengths of leg segments, in mm + 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) + self.x_dir = x_dir + self.y_dir = y_dir + self.home = (self.coxa + self.femur) / math.sqrt(2) + self.x = self.home + self.y = self.home + self.z = -self.tibia + + def move_to(self, 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) - PI4 + knee = _solve_triangle(self.femur, d, self.tibia) - math.atan2(-z, f) + ankle = _solve_triangle(self.femur, self.tibia, d) - PI2 + for servo, angle in zip(self.servos, (hip, knee, ankle)): + servo.move(angle) + self.x = x + self.y = y + self.z = z + + def move_by(self, dx=0, dy=0, dz=0): + self.move_to(self.x + dx, self.y + dy, self.z + dz) + + def rotate_by(self, radians): + x = self.x + self.base + y = self.y + self.base + nx = x * math.cos(radians) - y * math.sin(radians) - self.base + ny = x * math.sin(radians) + y * math.cos(radians) - self.base + self.move_to(nx, ny, self.z) + + +class Robot: + def __init__(self): + self.legs = ( + Leg(Servo('Y3', 0), Servo( 'Y7', 0), Servo('Y8', 1), -1, 1), + Leg(Servo('Y4', 1), Servo('X10', 1), Servo('X9', 0), -1, -1), + Leg(Servo('X3', 0), Servo( 'X7', 0), Servo('X8', 1), 1, -1), + Leg(Servo('X4', 1), Servo('Y10', 1), Servo('Y9', 0), 1, 1), + ) + + def move_by(self, dx=0, dy=0, dz=0): + for leg in self.legs: + leg.move_by(dx * leg.x_dir, dy * leg.y_dir, dz) + + def rotate_by(self, radians): + for leg in self.legs: + leg.rotate_by(radians)
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/servo.py Sat Dec 19 22:02:01 2015 +0100 @@ -0,0 +1,50 @@ +# -*- coding: utf-8 -*- + +import math +import pyb + + +PI2 = math.pi / 2 +TAU = math.pi * 2 +TIMERS = { + # pin: timer, channel + 'Y8': (12, 2), + 'Y7': (12, 1), + 'Y3': (4, 3), + 'X9': (4, 1), + 'X10': (4, 2), + 'Y4': (4, 4), + 'X8': (14, 1), + 'X7': (13, 1), + 'X3': (5, 3), + 'Y9': (2, 3), + 'Y10': (2, 4), + 'X4': (5, 4), +} + + +class Servo: + freq_min = 600 + freq_max = 2400 + + def __init__(self, pin, reverse=False, trim=0): + timer, channel = TIMERS[pin] + self.timer = pyb.Timer(timer, prescaler=83, period=19999) + self.channel = self.timer.channel(channel, pyb.Timer.PWM, + pin=pyb.Pin(pin)) + self.reverse = -1 if reverse else 1 + self.trim = trim + + def move(self, radians): + # Move a servo to a position in radians between -PI/2 and PI/2 + rads = radians * self.reverse + PI2 + while rads > TAU: + rads -= TAU + while rads < 0: + rads += TAU + freq = (self.freq_max - self.freq_min) * rads / math.pi + self.freq_min + freq = min(self.freq_max, max(self.freq_min, freq + self.trim)) + self.channel.pulse_width(int(freq)) + + def __del__(self): + self.timer.deinit()