Mercurial > tote-had
changeset 7:6417d860a1ca
Add gait.py with the basic creep
| author | Radomir Dopieralski <sheep-devel@sheep.art.pl> |
|---|---|
| date | Sat, 26 Mar 2016 22:53:03 +0100 |
| parents | bfbc645e9dea |
| children | 7993a8f67e4c |
| files | controller/leg.h fritzing/Makefile fritzing/pcb-had.fzz fritzing/pcb-had.zip python/gait.py python/robot.py |
| diffstat | 6 files changed, 120 insertions(+), 17 deletions(-) [+] |
line wrap: on
line diff
--- a/controller/leg.h Sun Mar 20 17:31:20 2016 +0100 +++ b/controller/leg.h Sat Mar 26 22:53:03 2016 +0100 @@ -1,19 +1,10 @@ #ifndef LEG_H #define LEG_H -#ifdef TOTE2 -// For version 2: #define COXA 25.0 #define FEMUR 42.0 #define TIBIA 38.5 #define BASE 21.0 -#else -// For version 1: -#define COXA 8.0 -#define FEMUR 20.5 -#define TIBIA 28.5 -#define BASE 18.5 -#endif const signed char LEG_X[4] = {-1, -1, 1, 1}; const signed char LEG_Y[4] = {1, -1, -1, 1};
--- a/fritzing/Makefile Sun Mar 20 17:31:20 2016 +0100 +++ b/fritzing/Makefile Sat Mar 26 22:53:03 2016 +0100 @@ -1,5 +1,5 @@ FRITZING=fritzing -PCBS=pcb2.zip pcb3.zip pcb4.zip +PCBS=pcb-had.zip all: $(PCBS)
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/python/gait.py Sat Mar 26 22:53:03 2016 +0100 @@ -0,0 +1,72 @@ +from machine import delay + + +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) + 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]
--- a/python/robot.py Sun Mar 20 17:31:20 2016 +0100 +++ b/python/robot.py Sat Mar 26 22:53:03 2016 +0100 @@ -1,19 +1,59 @@ from machine import I2C, Pin import ustruct +import math + + +I2C_ADDRESS = 0x09 -class Robot: - I2C_ADDRESS = 0x09 +class Leg: + base = 70 # cm from the center of the robot to the (0,0) of the leg - def __init__(self): - self.i2c = I2C(sda=Pin(0), scl=Pin(2)) + def __init__(self, i2c, index, x_dir, y_dir): + self.index = index + self.i2c = i2c + self.x = 0 + self.y = 0 + self.z = 0 - def leg_move_to(self, leg, x, y, z): - data = ustruct.pack("Bbbbb", 4 * leg, 1, x, y, z) + def move_to(self, x, y, z): + data = ustruct.pack("Bbbbb", 4 * self.index, 1, x, y, z) while True: try: - self.i2c.writeto(self.I2C_ADDRESS, data) + self.i2c.writeto(I2C_ADDRESS, data) except OSError: pass else: break + 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.i2c = I2C(sda=Pin(0), scl=Pin(2)) + self.legs = ( + Leg(self.i2c, 0, -1, 1), + Leg(self.i2c, 1, -1, -1), + Leg(self.i2c, 2, 1, -1), + Leg(self.i2c, 3, 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)