Compare commits
2 Commits
55b468d314
...
8484264a65
Author | SHA1 | Date | |
---|---|---|---|
8484264a65 | |||
b968dbe03c |
|
@ -1,2 +0,0 @@
|
|||
esptool.py --port /dev/ttyUSB0 write_flash -fm qio 0x00000 nodemcu-master-9-modules-2019-04-08-19-43-38-float.bin
|
||||
|
|
@ -1,46 +0,0 @@
|
|||
dofile('motor.lua')
|
||||
|
||||
cfg={}
|
||||
cfg.ssid="robot-1"
|
||||
cfg.pwd="roborally"
|
||||
wifi.ap.config(cfg)
|
||||
|
||||
sv = net.createServer()
|
||||
|
||||
Peer = {}
|
||||
function Peer:create(sck)
|
||||
local peer = {}
|
||||
setmetatable(peer, Peer)
|
||||
peer.buffer = {}
|
||||
return peer
|
||||
end
|
||||
function Peer:receive(sck, data)
|
||||
self.buffer = self.buffer .. data
|
||||
if #self.buffer >= 4 then
|
||||
dir = string.byte(self.buffer,1)
|
||||
if dir > 127 then dir = -256+dir end
|
||||
motor_dir(1, dir)
|
||||
|
||||
dir = string.byte(self.buffer,2)
|
||||
if dir > 127 then dir = -256+dir end
|
||||
motor_dir(2, dir)
|
||||
|
||||
delay = string.byte(self.buffer,3) * 256 + string.byte(self.buffer, 4)
|
||||
tmr.delay(delay)
|
||||
|
||||
motor_dir(1, 0)
|
||||
motor_dir(2, 0)
|
||||
self.buffer = {}
|
||||
sck:close()
|
||||
end
|
||||
end
|
||||
|
||||
if sv then
|
||||
-- listen to port 80 and call callback function if data is received
|
||||
sv:listen(80,
|
||||
function(conn)
|
||||
peer = Peer:create(conn)
|
||||
conn:on("receive", peer:receive(conn))
|
||||
conn:send("robot-1 at your service")
|
||||
end)
|
||||
end
|
|
@ -1,28 +0,0 @@
|
|||
motor = {}
|
||||
motor[1] = {}
|
||||
motor[1][1] = 0
|
||||
motor[1][2] = 1
|
||||
motor[1][3] = 2
|
||||
|
||||
motor[2] = {}
|
||||
motor[2][1] = 3
|
||||
motor[2][2] = 4
|
||||
motor[2][3] = 5
|
||||
|
||||
for i,m in ipairs(motor) do
|
||||
for i,g in ipairs(m) do
|
||||
gpio.mode(g, gpio.OUTPUT)
|
||||
gpio.write(g, 1)
|
||||
end
|
||||
end
|
||||
|
||||
function motor_dir(m, dir)
|
||||
if dir ~= 0 then
|
||||
gpio.write(motor[m][3], 1)
|
||||
gpio.write(motor[m][1], (1-dir)/2)
|
||||
gpio.write(motor[m][2], (1+dir)/2)
|
||||
else
|
||||
gpio.write(motor[m][3], 0)
|
||||
end
|
||||
end
|
||||
|
|
@ -1,28 +0,0 @@
|
|||
motor = {}
|
||||
motor[1] = {}
|
||||
motor[1][1] = 0
|
||||
motor[1][2] = 1
|
||||
motor[1][3] = 2
|
||||
|
||||
motor[2] = {}
|
||||
motor[2][1] = 3
|
||||
motor[2][2] = 4
|
||||
motor[2][3] = 5
|
||||
|
||||
for i,m in ipairs(motor) do
|
||||
for i,g in ipairs(m) do
|
||||
gpio.mode(g, gpio.OUTPUT)
|
||||
gpio.write(g, 1)
|
||||
end
|
||||
end
|
||||
|
||||
function motor_dir(m, dir)
|
||||
if dir ~= 0 then
|
||||
gpio.write(motor[m][3], 1)
|
||||
gpio.write(motor[m][1], (1-dir)/2)
|
||||
gpio.write(motor[m][2], (1+dir)/2)
|
||||
else
|
||||
gpio.write(motor[m][3], 0)
|
||||
end
|
||||
end
|
||||
|
|
@ -1,47 +0,0 @@
|
|||
motor = {}
|
||||
motor[1] = {}
|
||||
motor[1][1] = 0
|
||||
motor[1][2] = 1
|
||||
motor[1][3] = 2
|
||||
|
||||
motor[2] = {}
|
||||
motor[2][1] = 3
|
||||
motor[2][2] = 4
|
||||
motor[2][3] = 5
|
||||
|
||||
for i,m in ipairs(motor) do
|
||||
for i,g in ipairs(m) do
|
||||
gpio.mode(g, gpio.OUTPUT)
|
||||
gpio.write(g, 1)
|
||||
end
|
||||
end
|
||||
|
||||
function motor_dir(m, dir)
|
||||
if dir ~= 0 then
|
||||
gpio.write(motor[m][3], 1)
|
||||
gpio.write(motor[m][1], (1-dir)/2)
|
||||
gpio.write(motor[m][2], (1+dir)/2)
|
||||
else
|
||||
gpio.write(motor[m][3], 0)
|
||||
end
|
||||
end
|
||||
|
||||
Peer = {}
|
||||
function Peer:create(sck)
|
||||
local peer = {}
|
||||
setmetatable(peer, Peer)
|
||||
peer.buffer = {}
|
||||
return peer
|
||||
end
|
||||
function Peer:receive(sck, data)
|
||||
self.buffer = self.buffer .. data
|
||||
if #self.buffer >= 4 then
|
||||
|
||||
sck:close()
|
||||
end
|
||||
end
|
||||
|
||||
function(conn)
|
||||
peer = Peer:create(conn)
|
||||
conn:on("receive", peer:receive)
|
||||
end
|
Binary file not shown.
Binary file not shown.
|
@ -1,12 +0,0 @@
|
|||
# flashing firmware onto NodeMCU
|
||||
esptool.py --port /dev/ttyUSB0 write_flash -fm qio 0x00000 nodemcu-master-9-modules-2019-04-08-19-43-38-float.bin
|
||||
|
||||
# connecting to NodeMCU via serial terminal
|
||||
cu -l /dev/ttyUSB0 -s 115200 dir
|
||||
|
||||
# uploading code to NodeMCU
|
||||
nodemcu-uploader upload motor.lua
|
||||
nodemcu-uploader upload init.lua
|
||||
|
||||
# sending data over wifi via socket
|
||||
nc 192.168.4.1 80
|
|
@ -1 +0,0 @@
|
|||
cu -l /dev/ttyUSB0 -s 115200 dir
|
|
@ -1,2 +0,0 @@
|
|||
nodemcu-uploader upload motor.lua
|
||||
nodemcu-uploader upload init.lua
|
|
@ -1,9 +0,0 @@
|
|||
https://hackaday.com/2016/06/14/hackaday-prize-entry-micro-robots-for-education/
|
||||
|
||||
https://www.adafruit.com/product/3216
|
||||
|
||||
https://www.exp-tech.de/plattformen/robotik/roboterfahrzeuge/7673/adafruit-mini-round-robot-chassis-kit-2wd-with-dc-motors?gclid=EAIaIQobChMIrpHIq4Ke4QIVSeAYCh0LTgdWEAQYCCABEgIQcfD_BwE
|
||||
|
||||
https://www.exp-tech.de/plattformen/robotik/roboterfahrzeuge/7898/adafruit-mini-3-layer-round-robot-chassis-kit-2wd-with-dc-motors?gclid=EAIaIQobChMIrpHIq4Ke4QIVSeAYCh0LTgdWEAQYCyABEgI76vD_BwE
|
||||
|
||||
|
|
@ -1,43 +0,0 @@
|
|||
import socket
|
||||
import pygame
|
||||
|
||||
pygame.init()
|
||||
pygame.display.set_mode((640, 480))
|
||||
|
||||
rc_socket = socket.socket()
|
||||
try:
|
||||
rc_socket.connect(('127.0.0.1', 1337)) # connect to robot
|
||||
except socket.error:
|
||||
print("could not connect to socket")
|
||||
|
||||
|
||||
while True:
|
||||
u1 = 0
|
||||
u2 = 0
|
||||
vmax = 10.0
|
||||
events = pygame.event.get()
|
||||
for event in events:
|
||||
if event.type == pygame.KEYDOWN:
|
||||
if event.key == pygame.K_LEFT:
|
||||
u1 = -vmax / 10.0
|
||||
u2 = vmax/ 10.0
|
||||
|
||||
print("turn left: ({},{})".format(u1, u2))
|
||||
elif event.key == pygame.K_RIGHT:
|
||||
u1 = vmax/ 10.0
|
||||
|
||||
u2 = -vmax/ 10.0
|
||||
|
||||
print("turn right: ({},{})".format(u1, u2))
|
||||
elif event.key == pygame.K_UP:
|
||||
u1 = vmax
|
||||
u2 = vmax
|
||||
print("forward: ({},{})".format(u1, u2))
|
||||
elif event.key == pygame.K_DOWN:
|
||||
u1 = -vmax
|
||||
u2 = -vmax
|
||||
print("backward: ({},{})".format(u1, u2))
|
||||
rc_socket.send('({},{})\n'.format(u1, u2))
|
||||
elif event.type == pygame.KEYUP:
|
||||
print("key released, resetting: ({},{})".format(u1, u2))
|
||||
rc_socket.send('({},{})\n'.format(u1, u2))
|
|
@ -1,180 +0,0 @@
|
|||
import sys
|
||||
import socket
|
||||
import time
|
||||
import numpy as np
|
||||
|
||||
import pygame
|
||||
|
||||
from threading import Thread, Lock
|
||||
|
||||
import scipy.integrate
|
||||
|
||||
single_robot = True
|
||||
|
||||
class Server:
|
||||
def __init__(self):
|
||||
self.ip = '127.0.0.1'
|
||||
self.recv_port = 1337 # port for receiving data
|
||||
self.send_port = 1338 # port for sending data
|
||||
self.recv_addr = socket.getaddrinfo(self.ip, self.recv_port)[0][-1]
|
||||
|
||||
if not single_robot:
|
||||
self.robots = []
|
||||
else:
|
||||
self.rob = Robot()
|
||||
|
||||
self.receiver = Thread(target=self.remote_control)
|
||||
self.receiver.start()
|
||||
|
||||
def remote_control(self):
|
||||
print("setting up socket communication ...")
|
||||
recv_socket = socket.socket()
|
||||
recv_socket.bind(self.recv_addr)
|
||||
while True:
|
||||
print("waiting for connections on {} ...".format(self.recv_addr))
|
||||
recv_socket.listen(2)
|
||||
(comm_socket, address) = recv_socket.accept() # this blocks until someone connects to the socket
|
||||
|
||||
print("starting thread for handling communication with {}".format(address))
|
||||
Thread(target=self.handle_communication, args=(comm_socket, address)).start()
|
||||
|
||||
def handle_communication(self, socket, address):
|
||||
print("connected!")
|
||||
listening = True
|
||||
|
||||
if not single_robot:
|
||||
rob = Robot()
|
||||
self.robots.append(rob)
|
||||
|
||||
try:
|
||||
while listening:
|
||||
# expected data: '(u1, u2)'\n"
|
||||
# where ui = control for motor i
|
||||
# ui \in [-1.0, 1.0]
|
||||
data = socket.recv(4096)
|
||||
data_str = data.decode()
|
||||
#print("Data received: {}".format(data_str))
|
||||
#print("processing data = {}".format(data_str))
|
||||
l = data_str.strip('()\n').split(',')
|
||||
#print("l = {}".format(l))
|
||||
u1 = float(l[0])
|
||||
#print("u1 = {}".format(u1))
|
||||
u2 = float(l[1])
|
||||
#print("u2 = {}".format(u2))
|
||||
|
||||
if single_robot:
|
||||
self.rob.set_control(u1, u2)
|
||||
else:
|
||||
rob.set_control(u1, u2)
|
||||
except ValueError:
|
||||
print("ValueError: Data has wrong format.")
|
||||
print("Data received: {}".format(data_str))
|
||||
except IndexError:
|
||||
print("IndexError: Data has wrong format.")
|
||||
print("Data received: {}".format(data_str))
|
||||
except Exception as e:
|
||||
print("Some other error occured")
|
||||
print("Exception: {}".format(e))
|
||||
finally:
|
||||
socket.close()
|
||||
if not single_robot:
|
||||
self.robots.remove(rob)
|
||||
|
||||
class Robot:
|
||||
def __init__(self):
|
||||
self.x = np.array([0.0, 0.0, 0.0])
|
||||
|
||||
self.u1 = 0.0
|
||||
self.u2 = 0.0
|
||||
|
||||
self.r = 0.3
|
||||
self.R = 0.5
|
||||
self.d = 0.2
|
||||
|
||||
self.mutex = Lock()
|
||||
|
||||
self.omega_max = 50.0
|
||||
|
||||
self.integrator = scipy.integrate.ode(self.f)
|
||||
self.integrator.set_integrator('dopri5')
|
||||
|
||||
self.simulator = Thread(target=self.simulate)
|
||||
self.simulator.start()
|
||||
|
||||
def set_control(self, u1, u2):
|
||||
self.mutex.acquire()
|
||||
try:
|
||||
self.u1 = u1
|
||||
self.u2 = u2
|
||||
finally:
|
||||
self.mutex.release()
|
||||
|
||||
def simulate(self):
|
||||
t0 = time.time()
|
||||
t = time.time()-t0
|
||||
|
||||
while True:
|
||||
self.mutex.acquire()
|
||||
try:
|
||||
self.integrator.set_f_params(self.omega_max * np.array([self.u1, self.u2]))
|
||||
finally:
|
||||
self.mutex.release()
|
||||
self.integrator.set_initial_value(self.x, t)
|
||||
|
||||
dt = time.time() - t0 - t
|
||||
|
||||
self.x = self.integrator.integrate(self.integrator.t + dt)
|
||||
t = time.time() - t0
|
||||
|
||||
def f(self, t, x, u):
|
||||
r = self.r
|
||||
d = self.d
|
||||
R = self.R
|
||||
|
||||
theta = x[2]
|
||||
omegar = u[0]
|
||||
omegal = u[1]
|
||||
|
||||
dx = np.zeros((3,1))
|
||||
dx[0] = (r/2.0 * np.cos(theta) - r * d/(2.0 * R) * np.sin(theta)) * omegar + \
|
||||
(r/2.0 * np.cos(theta) + r * d/(2.0 * R) * np.sin(theta)) * omegal
|
||||
dx[1] = (r/2.0 * np.sin(theta) + r * d/(2.0 * R) * np.cos(theta)) * omegar + \
|
||||
(r/2.0 * np.sin(theta) - r * d/(2.0 * R) * np.cos(theta)) * omegal
|
||||
dx[2] = r/(2.0 * R) * (omegar - omegal)
|
||||
|
||||
return dx
|
||||
|
||||
def main(args):
|
||||
# wait for connections of socket
|
||||
s = Server()
|
||||
|
||||
background_colour = (255, 255, 255)
|
||||
(width, height) = (640, 480)
|
||||
screen = pygame.display.set_mode((width, height))
|
||||
pygame.display.set_caption('Roboflut')
|
||||
screen.fill(background_colour)
|
||||
pygame.display.flip()
|
||||
|
||||
running = True
|
||||
while running:
|
||||
r = s.rob
|
||||
#for r in s.robots:
|
||||
x = int(r.x[0] + width/2.0)
|
||||
y = int(r.x[1] + height / 2.0)
|
||||
phi = r.x[2]
|
||||
phases = ( phi + np.pi) % (2 * np.pi ) - np.pi
|
||||
|
||||
if x < 0 or x > width or y < 0 or y > height:
|
||||
r.x[0] = 0.0
|
||||
r.x[1] = 0.0
|
||||
c = int((phases + 2.0 * np.pi) / (4.0 * np.pi) * 255)
|
||||
#color = (c, c, c)
|
||||
color = (0,0,0)
|
||||
|
||||
screen.set_at((x, y), color)
|
||||
|
||||
pygame.display.flip()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main(sys.argv)
|
Loading…
Reference in New Issue
Block a user