Mercurial > circuitpython-spiderwing
changeset 3:8ed8f30b0568 draft
Update gaits
| author | Radomir Dopieralski <sheep-devel@sheep.art.pl> |
|---|---|
| date | Sun, 23 Jul 2017 20:23:29 +0200 |
| parents | 2f1609829f66 |
| children | 48cead9d49b5 |
| files | crab/gait.py crab/hardware.py crab/main.py crab/robot.py python/gait.py python/robot.py |
| diffstat | 6 files changed, 103 insertions(+), 35 deletions(-) [+] |
line wrap: on
line diff
--- a/crab/gait.py Mon May 15 20:01:55 2017 +0200 +++ b/crab/gait.py Sun Jul 23 20:23:29 2017 +0200 @@ -3,12 +3,14 @@ class Creep: - speed = 0.5 - stride = 15 - shift = 8 - shift_steps = 2 + speed = 0.3 + stride = 12 + shift = 12 + shift_steps = 3 raise_leg = 10 raise_steps = 2 + spread = -10 + height = 5 next_leg = ( (2, 0, 3, 1), # forward (1, 3, 0, 2), # backward @@ -23,11 +25,9 @@ self.shift_x = 0 self.shift_y = 0 self.on_ground = list(robot.legs) - self.dx = 1 + self.dx = 0 self.dy = 0 self.rotation = 0 - self.spread = -10 - self.height = 5 self.last_leg = 0 def move(self): @@ -35,7 +35,7 @@ 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 * self.speed) + leg.rotate_by(-self.rotation * self.speed) time.sleep(0.001) def step(self, leg): @@ -57,7 +57,7 @@ (self.shift_y + self.dy * self.stride) * leg.y_dir + self.spread, self.raise_leg * self.raise_steps + self.height ) - leg.rotate_by(self.rotation * leg.x_dir * leg.y_dir * self.stride / 10) + leg.rotate_by(self.rotation * self.stride / 10) self.move() self.move() for step in range(self.raise_steps - 1):
--- a/crab/hardware.py Mon May 15 20:01:55 2017 +0200 +++ b/crab/hardware.py Sun Jul 23 20:23:29 2017 +0200 @@ -13,6 +13,7 @@ self._i2c_buffer = bytearray(2) self._init_servo() self._init_eyes() + self._init_sensor() #self._init_voice() def _i2c_reg(self, address, register, value=None): @@ -54,6 +55,11 @@ finally: self.i2c.unlock() + def _init_sensor(self): + self._i2c_reg(0x60, 0xee, 0x06) + #self._i2c_reg(0x60, 0x3f, 0x00) + #self._i2c_reg(0x60, 0xa8, 0x03) + def sensor(self): self._i2c_buffer[0] = 0x5e self.i2c.try_lock() @@ -64,13 +70,13 @@ self.i2c.unlock() if self._i2c_buffer[0] == 0xff: return 0 - return (self._i2c_buffer[0] << 4 | self._i2c_buffer[1]) / 3.2 + return ((self._i2c_buffer[0] << 4) | self._i2c_buffer[1]) / 3.2 def _init_eyes(self): self._eye_pins = ( digitalio.DigitalInOut(board.D9), + digitalio.DigitalInOut(board.D11), digitalio.DigitalInOut(board.D10), - digitalio.DigitalInOut(board.D11), ) for pin in self._eye_pins: pin.switch_to_output()
--- a/crab/main.py Mon May 15 20:01:55 2017 +0200 +++ b/crab/main.py Sun Jul 23 20:23:29 2017 +0200 @@ -1,5 +1,73 @@ +import math +import time from hardware import Hardware from robot import Robot +from gait import Creep +import samd + +samd.disable_autoreload() robot = Robot(Hardware()) robot.unfold() +gait = Creep(robot) +gait.dx = 0 + +dr = math.pi / 80 +while 1: + try: + distance = robot.hardware.sensor() + if distance == 0: + robot.hardware.eye_color(4) + gait.dx = 0 + i = 10 + j = 0 + while distance == 0: + robot.rotate_by(dr) + i += 1 + if i > 20: + i = 0 + dr = -dr + j += 1 + if j > 4: + j = 0 + robot.hardware.eye_color(0) + robot.fold() + while distance == 0: + time.sleep(1) + distance = robot.hardware.sensor() + robot.unfold() + time.sleep(0.2) + distance = robot.hardware.sensor() + elif distance < 200: + robot.hardware.eye_color(1) + gait.dx = -1 + elif distance < 250: + robot.hardware.eye_color(5) + gait.dx = 0 + time.sleep(1) + distance2 = robot.hardware.sensor() + if abs(distance - distance2) < 5: + robot.rotate_by(dr) + time.sleep(0.1) + distance = robot.hardware.sensor() + robot.rotate_by(-dr) + time.sleep(0.1) + robot.rotate_by(-dr) + time.sleep(0.1) + distance2 = robot.hardware.sensor() + robot.rotate_by(dr) + time.sleep(0.1) + if distance2 > distance: + dr = -dr + robot.hardware.eye_color(6) + gait.rotation = dr + gait.run(10) + gait.rotation = 0 + else: + distance = distance2 + else: + robot.hardware.eye_color(3) + gait.dx = 1 + gait.run(1) + except ValueError: + robot.home()
--- a/crab/robot.py Mon May 15 20:01:55 2017 +0200 +++ b/crab/robot.py Sun Jul 23 20:23:29 2017 +0200 @@ -25,6 +25,7 @@ self.move_to(self.x + dx, self.y + dy, self.z + dz) def rotate_by(self, radians): + radians *= self.x_dir * self.y_dir x = self.x + ik.BASE + ik.HOME y = self.y + ik.BASE + ik.HOME nx = x * math.cos(radians) - y * math.sin(radians) - ik.BASE - ik.HOME @@ -48,7 +49,7 @@ def rotate_by(self, radians): for leg in self.legs: - leg.rotate_by(radians * leg.x_dir * leg.y_dir) + leg.rotate_by(radians) def home(self): for leg in self.legs:
--- a/python/gait.py Mon May 15 20:01:55 2017 +0200 +++ b/python/gait.py Sun Jul 23 20:23:29 2017 +0200 @@ -2,12 +2,12 @@ class Creep: - speed = 2 + speed = 2.3 stride = 36 - shift = 12 - shift_steps = 2 - raise_leg = 10 - raise_steps = 2 + shift = 8 + shift_steps = 1 + raise_leg = 30 + raise_steps = 1 next_leg = ( (2, 0, 3, 1), # forward (1, 3, 0, 2), # backward @@ -25,35 +25,31 @@ self.dx = 1 self.dy = 0 self.rotation = 0 - self.spread = 20 - leg = robot.legs[0] - self.height = 20 + self.spread = 0 + self.raise_spread = 10 + self.height = 30 self.last_leg = 0 def move(self): - print("Moving legs backwards") 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) + leg.rotate_by(-self.rotation * leg.x_dir * leg.y_dir / 2) utime.sleep_ms(10) 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 - print("Shifting body by (%d, %d) %d times" % (dx, dy, 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) - print("Raising leg by %d %d times" % (self.raise_leg, self.raise_steps)) for step in range(self.raise_steps): - leg.move_by(0, 0, self.raise_leg) + leg.move_by(self.raise_spread, self.raise_spread, self.raise_leg) self.move() - print("Moving leg forward") leg.move_to( (self.shift_x + self.dx * self.stride) * leg.x_dir + self.spread, (self.shift_y + self.dy * self.stride) * leg.y_dir + self.spread, @@ -61,17 +57,14 @@ ) self.move() self.move() - print("Lowering leg") 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() - print("Leg done") def run(self, steps): for step in range(steps): - print("Moving leg %d" % self.last_leg) leg = self.robot.legs[self.last_leg] self.step(leg) self.last_leg = self.next_leg[0][self.last_leg]
--- a/python/robot.py Mon May 15 20:01:55 2017 +0200 +++ b/python/robot.py Sun Jul 23 20:23:29 2017 +0200 @@ -25,10 +25,10 @@ class Leg: # Lengths of leg segments, in mm - coxa = 36 # between the hip and the knee - femur = 39 # between the knee and the ankle - tibia = 55 # between the ankle and the foot - base = 21 # from the center of body + coxa = 12.3 # between the hip and the knee + femur = 39.0 # between the knee and the ankle + tibia = 55.0 # between the ankle and the foot + base = 21.0 # from the center of body def __init__(self, joints, direction, servos): self.servos = servos @@ -40,8 +40,8 @@ self.z = 0 def move_to(self, x, y, z): - print("Moving leg %d, %d to %d, %d, %d" % (self.x_dir, self.y_dir, - x, y, z)) + print("Moving leg %d, %d to %d, %d, %d" % + (self.x_dir, self.y_dir, x, y, z)) x += self.home y += self.home z -= self.tibia