Mercurial > pkubik
changeset 4:24c33455723d
convert to micropython
| author | Radomir Dopieralski <sheep-devel@sheep.art.pl> |
|---|---|
| date | Tue, 25 Nov 2014 22:59:41 +0100 |
| parents | f0c05b8506be |
| children | 17634205b35c |
| files | robot.py robot.sh |
| diffstat | 2 files changed, 19 insertions(+), 4 deletions(-) [+] |
line wrap: on
line diff
--- a/robot.py Mon Nov 24 00:35:13 2014 +0100 +++ b/robot.py Tue Nov 25 22:59:41 2014 +0100 @@ -35,6 +35,7 @@ positions = [int(pos * 1000) for pos in self.positions] packet = struct.pack(format, command, *positions) self.serial.write(packet) + self.serial.flush() def move_servo(self, servo, radians=None, degrees=None): if degrees is not None: @@ -77,8 +78,13 @@ self.leg_positions = [leg.home for leg in self.legs] self.shift = [0, 0, 0] - def _inverse_kinematics(self, (x, y, z), (dx, dy)): + def _inverse_kinematics(self, pos, d): """Calculate the 3 angles of the leg, in degrees.""" + # Micropython doesn't support tuple unpacking? + x = pos[0] + y = pos[1] + z = pos[2] + dy = d[1] f = _norm(x, y) - self.coxa d = min(self.femur + self.tibia, _norm(f, z)) hip = math.atan2(y, x) @@ -125,12 +131,17 @@ self.move_leg_to(leg, (rx, -ry * y_dir, None)) +def make_robot(serial_device='/dev/ttyS0'): + serial = open(serial_device, 'w') + servos = ServoController(serial) + robot = Robot(servos) + return robot + + if __name__ == '__main__': - import serial import time - servos = ServoController(serial.Serial('/dev/ttyUSB0', 57600)) - robot = Robot(servos) + robot = make_robot() robot.update() time.sleep(1) steps = 10
