Mercurial > pony
changeset 5:c1b6dd72fb3c
Add leg.py and add upload to the makefile
| author | Radomir Dopieralski <sheep-devel@sheep.art.pl> |
|---|---|
| date | Tue, 10 May 2016 17:56:10 +0200 |
| parents | 1ced6f4ec07d |
| children | 1529d6a90448 |
| files | .hgignore Makefile leg.py tools/webrepl_cli.py tools/websocket_helper.py |
| diffstat | 5 files changed, 342 insertions(+), 1 deletions(-) [+] |
line wrap: on
line diff
--- a/.hgignore Sun May 08 16:34:13 2016 +0200 +++ b/.hgignore Tue May 10 17:56:10 2016 +0200 @@ -2,6 +2,7 @@ *~ .*.swp *.DS_Store +*.pyc doc/_build pony.zip pony.pdf
--- a/Makefile Sun May 08 16:34:13 2016 +0200 +++ b/Makefile Tue May 10 17:56:10 2016 +0200 @@ -1,7 +1,17 @@ FRITZING=fritzing PCBS=pony.zip +PYSRC=leg.py~ servo.py~ +ESP_ADDR=192.168.0.17 -all: $(PCBS) +all: + +upload: $(PYSRC) + +%.py~: %.py + python tools/webrepl_cli.py $< $(ESP_ADDR): + touch $@ + +pcbs: $(PCBS) clean: rm -f $(PCBS)
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/leg.py Tue May 10 17:56:10 2016 +0200 @@ -0,0 +1,35 @@ +import math +from machine import Pin + +from servo import Servo + + +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: + femur = 21 # mm + tibia = 27 # mm + + def __init__(self, hip_pin, knee_pin): + self.servos = (Servo(Pin(hip_pin)), Servo(Pin(knee_pin))) + + 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 + + +class HindLeg(Leg): + tibia = 35 # mm
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/tools/webrepl_cli.py Tue May 10 17:56:10 2016 +0200 @@ -0,0 +1,220 @@ +#!/usr/bin/env python +from __future__ import print_function +import sys +import os +import struct +import getpass +try: + import usocket as socket +except ImportError: + import socket +import websocket_helper + +# Define to 1 to use builtin "websocket" module of MicroPython +USE_BUILTIN_WEBSOCKET = 0 +# Treat this remote directory as a root for file transfers +SANDBOX = "" +#SANDBOX = "/tmp/webrepl/" +DEBUG = 0 + +WEBREPL_FILE = "<2sBBQLH64s" + + +def debugmsg(msg): + if DEBUG: + print(msg) + + +if USE_BUILTIN_WEBSOCKET: + from websocket import websocket +else: + class websocket: + + def __init__(self, s): + self.s = s + self.buf = b"" + + def write(self, data): + l = len(data) + if l < 126: + # TODO: hardcoded "binary" type + hdr = struct.pack(">BB", 0x82, l) + else: + hdr = struct.pack(">BBH", 0x82, 126, l) + self.s.send(hdr) + self.s.send(data) + + def recvexactly(self, sz): + res = b"" + while sz: + data = self.s.recv(sz) + if not data: + break + res += data + sz -= len(data) + return res + + def read(self, size, text_ok=False): + if not self.buf: + while True: + hdr = self.recvexactly(2) + assert len(hdr) == 2 + fl, sz = struct.unpack(">BB", hdr) + if sz == 126: + hdr = self.recvexactly(2) + assert len(hdr) == 2 + (sz,) = struct.unpack(">H", hdr) + if fl == 0x82: + break + if text_ok and fl == 0x81: + break + debugmsg("Got unexpected websocket record of type %x, skipping it" % fl) + while sz: + skip = self.s.recv(sz) + debugmsg("Skip data: %s" % skip) + sz -= len(skip) + data = self.recvexactly(sz) + assert len(data) == sz + self.buf = data + + d = self.buf[:size] + self.buf = self.buf[size:] + assert len(d) == size, len(d) + return d + + def ioctl(self, req, val): + assert req == 9 and val == 2 + + +def login(ws): + while True: + c = ws.read(1, text_ok=True) + if c == b":": + assert ws.read(1, text_ok=True) == b" " + break + passwd = getpass.getpass() + ws.write(passwd.encode("utf-8") + b"\r") + +def read_resp(ws): + data = ws.read(4) + sig, code = struct.unpack("<2sH", data) + assert sig == b"WB" + return code + +def put_file(ws, local_file, remote_file): + sz = os.stat(local_file)[6] + dest_fname = (SANDBOX + remote_file).encode("utf-8") + rec = struct.pack(WEBREPL_FILE, b"WA", 1, 0, 0, sz, len(dest_fname), dest_fname) + print(rec, len(rec)) + ws.write(rec[:10]) + ws.write(rec[10:]) + assert read_resp(ws) == 0 + with open(local_file, "rb") as f: + while True: + buf = f.read(1024) + if not buf: + break + ws.write(buf) + assert read_resp(ws) == 0 + +def get_file(ws, local_file, remote_file): + src_fname = (SANDBOX + remote_file).encode("utf-8") + rec = struct.pack(WEBREPL_FILE, b"WA", 2, 0, 0, 0, len(src_fname), src_fname) + print(rec, len(rec)) + ws.write(rec) + assert read_resp(ws) == 0 + with open(local_file, "wb") as f: + while True: + (sz,) = struct.unpack("<H", ws.read(2)) + if sz == 0: + break + while sz: + buf = ws.read(sz) + if not buf: + raise OSError() + f.write(buf) + sz -= len(buf) + assert read_resp(ws) == 0 + + +def help(rc=0): + exename = sys.argv[0].rsplit("/", 1)[-1] + print("%s - Perform remote file operations using MicroPython WebREPL protocol" % exename) + print("Arguments:") + print(" <host>:<remote_file> <local_file> - Copy remote file to local file") + print(" <local_file> <host>:<remote_file> - Copy local file to remote file") + print("Examples:") + print(" %s script.py 192.168.4.1:/another_name.py" % exename) + print(" %s script.py 192.168.4.1:/app/" % exename) + print(" %s 192.168.4.1:/app/script.py ." % exename) + sys.exit(rc) + +def error(msg): + print(msg) + sys.exit(1) + +def parse_remote(remote): + host, fname = remote.rsplit(":", 1) + if fname == "": + fname = "/" + port = 8266 + if ":" in host: + host, port = remote.split(":") + port = int(port) + return (host, port, fname) + +def main(): + + if len(sys.argv) != 3: + help(1) + + if ":" in sys.argv[1] and ":" in sys.argv[2]: + error("Operations on 2 remote files are not supported") + if ":" not in sys.argv[1] and ":" not in sys.argv[2]: + error("One remote file is required") + + if ":" in sys.argv[1]: + op = "get" + host, port, src_file = parse_remote(sys.argv[1]) + dst_file = sys.argv[2] + if os.path.isdir(dst_file): + basename = src_file.rsplit("/", 1)[-1] + dst_file += "/" + basename + else: + op = "put" + host, port, dst_file = parse_remote(sys.argv[2]) + src_file = sys.argv[1] + if dst_file[-1] == "/": + basename = src_file.rsplit("/", 1)[-1] + dst_file += basename + + if 1: + print(op, host, port) + print(src_file, "->", dst_file) + + s = socket.socket() + + ai = socket.getaddrinfo(host, port) + addr = ai[0][4] + + s.connect(addr) + #s = s.makefile("rwb") + websocket_helper.client_handshake(s) + + ws = websocket(s) + + login(ws) + + # Set websocket to send data marked as "binary" + ws.ioctl(9, 2) + + if op == "get": + get_file(ws, dst_file, src_file) + elif op == "put": + put_file(ws, src_file, dst_file) + + s.close() + + +if __name__ == "__main__": + main()
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/tools/websocket_helper.py Tue May 10 17:56:10 2016 +0200 @@ -0,0 +1,75 @@ +import sys +try: + import ubinascii as binascii +except: + import binascii +try: + import uhashlib as hashlib +except: + import hashlib + +DEBUG = 0 + +def server_handshake(sock): + clr = sock.makefile("rwb", 0) + l = clr.readline() + #sys.stdout.write(repr(l)) + + webkey = None + + while 1: + l = clr.readline() + if not l: + raise OSError("EOF in headers") + if l == b"\r\n": + break + # sys.stdout.write(l) + h, v = [x.strip() for x in l.split(b":", 1)] + if DEBUG: + print((h, v)) + if h == b'Sec-WebSocket-Key': + webkey = v + + if not webkey: + raise OSError("Not a websocket request") + + if DEBUG: + print("Sec-WebSocket-Key:", webkey, len(webkey)) + + respkey = webkey + b"258EAFA5-E914-47DA-95CA-C5AB0DC85B11" + respkey = hashlib.sha1(respkey).digest() + respkey = binascii.b2a_base64(respkey)[:-1] + + resp = b"""\ +HTTP/1.1 101 Switching Protocols\r +Upgrade: websocket\r +Connection: Upgrade\r +Sec-WebSocket-Accept: %s\r +\r +""" % respkey + + if DEBUG: + print(resp) + sock.send(resp) + + +# Very simplified client handshake, works for MicroPython's +# websocket server implementation, but probably not for other +# servers. +def client_handshake(sock): + cl = sock.makefile("rwb", 0) + cl.write(b"""\ +GET / HTTP/1.1\r +Host: echo.websocket.org\r +Connection: Upgrade\r +Upgrade: websocket\r +Sec-WebSocket-Key: foo\r +\r +""") + l = cl.readline() +# print(l) + while 1: + l = cl.readline() + if l == b"\r\n": + break +# sys.stdout.write(l)