Telos4/RoboRally
1
0
Fork 1
Code Issues Pull Requests Releases Wiki Activity

Compare commits

base: Telos4:15da18f949d12ab738f4fce28345c9025c2ab586
Branches Tags
Telos4:master
Telos4:simple_control
..
compare: Telos4:67405fbd3f49eebef0682d081c894a03ad8b2795
Branches Tags
Telos4:master
Telos4:simple_control

No commits in common. "15da18f949d12ab738f4fce28345c9025c2ab586" and "67405fbd3f49eebef0682d081c894a03ad8b2795" have entirely different histories.
15da18f949 ... 67405fbd3f

4 changed files with 63 additions and 236 deletions
Show Stats Expand all files Collapse all files

13
micropython_firmware/main.py
View File

@ -2,7 +2,7 @@ import machine
import sys import sys
from machine import I2C, Pin from machine import I2C, Pin
i2c = True i2c = False
if i2c: if i2c:
import d1motor import d1motor
else: else:
@ -13,13 +13,14 @@ import usocket
import esp import esp
class Robot: class Robot:
def __init__(self, i2c=True): def __init__(self):
if i2c: if i2c:
print("setting up I2C ...") print("setting up I2C ...")
d1 = Pin(5) #d1 = Pin(5)
d2 = Pin(4) #d2 = Pin(4)
i2c = I2C(scl=d1, sda=d2) #i2c = I2C(scl=d1, sda=d2)
i2c_scan = i2c.scan() #i2c_scan = i2c.scan()
i2c_scan = []
if len(i2c_scan) > 0: if len(i2c_scan) > 0:
i2c_addr = i2c_scan[0] i2c_addr = i2c_scan[0]
print("i2c scan = {}".format(i2c_addr)) print("i2c scan = {}".format(i2c_addr))

19
remote_control/casadi_opt.py
View File

@ -138,7 +138,7 @@ class OpenLoopSolver:
#return #return
def solve(self, x0, target, obstacles, track): def solve(self, x0, target, obstacles):
tstart = time.time() tstart = time.time()
# alternative solution using multiple shooting (way faster!) # alternative solution using multiple shooting (way faster!)
self.opti = Opti() # Optimization problem self.opti = Opti() # Optimization problem
@ -213,7 +213,7 @@ class OpenLoopSolver:
# ---- path constraints ----------- # ---- path constraints -----------
# limit = lambda pos: 1-sin(2*pi*pos)/2 # limit = lambda pos: 1-sin(2*pi*pos)/2
# self.opti.subject_to(speed<=limit(pos)) # track speed limit # self.opti.subject_to(speed<=limit(pos)) # track speed limit
maxcontrol = 0.9 maxcontrol = 0.95
self.opti.subject_to(self.opti.bounded(-maxcontrol, self.U, maxcontrol)) # control is limited self.opti.subject_to(self.opti.bounded(-maxcontrol, self.U, maxcontrol)) # control is limited
# ---- boundary conditions -------- # ---- boundary conditions --------
@ -236,17 +236,7 @@ class OpenLoopSolver:
if p is not None: if p is not None:
for k in range(1,self.N): for k in range(1,self.N):
self.opti.subject_to((self.X[0,k]-p[0])**2 + (self.X[1,k]-p[1])**2 + self.slack > r**2) self.opti.subject_to((self.X[0,k]-p[0])**2 + (self.X[1,k]-p[1])**2 + self.slack > r**2)
# keep inside track # pass
# TODO
# track_ids = track.inner.keys()
# a = track.outer[track_ids[0]]
# b = track.outer[track_ids[1]]
# c = track.outer[track_ids[2]]
# d = track.outer[track_ids[3]]
#
# for k in range(1, self.N):
# self.opti.subject_to(self.opti.subject_to(self.X))
posx = self.X[0, :] posx = self.X[0, :]
posy = self.X[1, :] posy = self.X[1, :]
angle = self.X[2, :] angle = self.X[2, :]
@ -260,11 +250,8 @@ class OpenLoopSolver:
tstart = time.time() tstart = time.time()
if self.use_warmstart and self.opti_x0 is not None: if self.use_warmstart and self.opti_x0 is not None:
try:
self.opti.set_initial(self.opti.lam_g, self.opti_lam_g0) self.opti.set_initial(self.opti.lam_g, self.opti_lam_g0)
self.opti.set_initial(self.opti.x, self.opti_x0) self.opti.set_initial(self.opti.x, self.opti_x0)
except RuntimeError:
print("could not set warmstart")
sol = self.opti.solve() # actual solve sol = self.opti.solve() # actual solve
tend = time.time() tend = time.time()
print("solving the problem took {} seconds".format(tend - tstart)) print("solving the problem took {} seconds".format(tend - tstart))

83
remote_control/class_control_joystick.py
View File

@ -13,34 +13,15 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va
self.robot0_stopped_3 = True self.robot0_stopped_3 = True
self.robot0_stopped_4 = True self.robot0_stopped_4 = True
self.rc_socket = socket.socket() self.rc_socket = socket.socket()
self.u1 = 0
self.u2 = 0
try: try:
self.rc_socket.connect((self.ip, self.port)) self.rc_socket.connect((self.ip, self.port))
except socket.error(): except socket.error():
print("couldn't connect to socket") print("couldn't connect to socket")
self.check_joystick = False
def joystick_init(self): # Joystick's initialisation def joystick_init(self): # Joystick's initialisation
joystick_count = pygame.joystick.get_count()
for count in range(joystick_count):
if joystick_count == 2:
self.joystick = pygame.joystick.Joystick(self.joy) self.joystick = pygame.joystick.Joystick(self.joy)
print("{}-->joystick count".format(joystick_count))
self.joystick.init() self.joystick.init()
self.check_joystick = True self.axes = self.joystick.get_numaxes()
elif joystick_count == 1:
joystick = pygame.joystick.Joystick(0)
joystick.init()
print("connected only 1 joystick - {}".format(joystick))
self.check_joystick = False
elif not joystick_count:
print("no joysticks connected")
self.check_joystick = False
def control(self, event): # the control of two robots with joysticks def control(self, event): # the control of two robots with joysticks
joy = event.joy joy = event.joy
@ -93,55 +74,54 @@ class robot: # we have 4 arg for this class, because joysticks get the same (va
self.robot0_stopped_4 = True self.robot0_stopped_4 = True
def control_keyboard(self, event): # keyboard control for robot1 def control_keyboard(self, event): # keyboard control for robot1
command_received = False
if pressed[pygame.K_LEFT]: if event.key == pygame.K_LEFT:
u1 = -1.0 u1 = -1.0
u2 = 1.0 u2 = 1.0
self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) command_received = True
elif event.key == pygame.K_RIGHT:
elif pressed[pygame.K_RIGHT]:
u1 = 1.0 u1 = 1.0
u2 = -1.0 u2 = -1.0
self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) command_received = True
elif event.key == pygame.K_UP:
elif pressed[pygame.K_UP]:
u1 = -1.0 u1 = -1.0
u2 = -1.0 u2 = -1.0
self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) command_received = True
elif event.key == pygame.K_DOWN:
elif pressed[pygame.K_DOWN]:
u1 = 1.0 u1 = 1.0
u2 = 1.0 u2 = 1.0
command_received = True
if command_received:
self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
elif event.type == pygame.KEYUP: def control_keyboard_2(self, event): # keyboard control for robot2
self.rc_socket.send('({},{})\n'.format(self.u1, self.u2).encode())
def control_keyboard_2(self, event): # keyboard control for robot1 command_received = False
if event.key == pygame.K_a:
if pressed[pygame.K_a]:
u1 = -1.0 u1 = -1.0
u2 = 1.0 u2 = 1.0
self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) command_received = True
elif event.key == pygame.K_d:
elif pressed[pygame.K_d]:
u1 = 1.0 u1 = 1.0
u2 = -1.0 u2 = -1.0
self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) command_received = True
elif event.key == pygame.K_w:
elif pressed[pygame.K_w]:
u1 = -1.0 u1 = -1.0
u2 = -1.0 u2 = -1.0
self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) command_received = True
elif event.key == pygame.K_s:
elif pressed[pygame.K_s]:
u1 = 1.0 u1 = 1.0
u2 = 1.0 u2 = 1.0
command_received = True
if command_received:
self.rc_socket.send('({},{})\n'.format(u1, u2).encode()) self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
elif event.type == pygame.KEYUP: def control_keyboard_stop(self): # stop for both robot
self.rc_socket.send('({},{})\n'.format(self.u1, self.u2).encode())
u1 = 0
u2 = 0
print("key released, resetting: ({},{})".format(u1, u2))
self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
def main(): def main():
pygame.init() pygame.init()
@ -151,18 +131,19 @@ def main():
robot_1.joystick_init() robot_1.joystick_init()
robot_2 = robot(1, '192.168.1.103', 1234) robot_2 = robot(1, '192.168.1.103', 1234)
robot_2.joystick_init() robot_2.joystick_init()
while True: while True:
events = pygame.event.get() events = pygame.event.get()
global pressed
pressed = pygame.key.get_pressed()
for event in events: for event in events:
if event.type == pygame.JOYAXISMOTION: if event.type == pygame.JOYAXISMOTION:
robot_1.control(event) robot_1.control(event)
robot_2.control(event) robot_2.control(event)
else: elif event.type == pygame.KEYDOWN:
robot_1.control_keyboard(event) robot_1.control_keyboard(event)
robot_2.control_keyboard_2(event) robot_2.control_keyboard_2(event)
elif event.type == pygame.KEYUP:
robot_1.control_keyboard_stop()
robot_2.control_keyboard_stop()
if __name__ == '__main__': if __name__ == '__main__':

176
remote_control/position_controller.py
View File

@ -20,16 +20,12 @@ import matplotlib.pyplot as plt
import matplotlib.animation as anim import matplotlib.animation as anim
import matplotlib.patches as patch import matplotlib.patches as patch
from shapely.geometry import Polygon
import time import time
from casadi_opt import OpenLoopSolver from casadi_opt import OpenLoopSolver
from marker_pos_angle.msg import id_pos_angle from marker_pos_angle.msg import id_pos_angle
from collections import OrderedDict
class Robot: class Robot:
def __init__(self, id, ip=None): def __init__(self, id, ip=None):
self.pos = None self.pos = None
@ -48,57 +44,6 @@ class Obstacle:
self.pos = None self.pos = None
self.radius = radius self.radius = radius
class Track:
def __init__(self):
# ids in clockwise direction
self.inner = OrderedDict()
self.inner[1] = None
self.inner[2] = None
self.inner[3] = None
self.inner[4] = None
self.outer = OrderedDict()
self.outer[5] = None
self.outer[6] = None
self.outer[7] = None
self.outer[8] = None
self.track_complete = False
self.inner_poly = None
self.outer_poly = None
def set_id(self, d):
if not self.track_complete:
if d.id in self.inner:
print("Detected marker {} at pos {}".format(d.id, (d.x,d.y)))
self.inner[d.id] = (d.x, d.y)
elif d.id in self.outer:
print("Detected marker {} at pos {}".format(d.id, (d.x, d.y)))
self.outer[d.id] = (d.x, d.y)
else:
print("Unknown marker!")
else:
return
if not None in self.inner.values() and not None in self.outer.values():
print("Track marker positions detected!")
self.track_complete = True
self.inner_poly = Polygon(self.inner.values())
self.outer_poly = Polygon(self.outer.values())
def plot_track(self):
if self.track_complete:
plt.figure(2)
x_in, y_in = self.inner_poly.exterior.xy
x_out, y_out = self.outer_poly.exterior.xy
plt.plot(x_in, y_in)
plt.plot(x_out, y_out)
else:
print("plot is not complete yet!")
def f_ode(t, x, u): def f_ode(t, x, u):
# dynamical model of the two-wheeled robot # dynamical model of the two-wheeled robot
# TODO: find exact values for these parameters # TODO: find exact values for these parameters
@ -124,21 +69,18 @@ def f_ode(t, x, u):
class RemoteController: class RemoteController:
def __init__(self): def __init__(self):
self.robots = [Robot(15, '192.168.1.103')] self.robots = [Robot(3, '192.168.1.103')]
#self.robots = [Robot(14, '192.168.1.102')]
self.robot_ids = {} self.robot_ids = {}
for r in self.robots: for r in self.robots:
self.robot_ids[r.id] = r self.robot_ids[r.id] = r
obst = [Obstacle(12, 0.275), Obstacle(10, 0.275), Obstacle(13, 0.275), Obstacle(14, 0.275)] obst = [Obstacle(6, 0.175), Obstacle(5, 0.175), Obstacle(8, 0.175)]
self.obstacles = {} self.obstacles = {}
for r in obst: for r in obst:
self.obstacles[r.id] = r self.obstacles[r.id] = r
self.track = Track()
# connect to robot # connect to robot
self.rc_socket = socket.socket() self.rc_socket = socket.socket()
#self.rc_socket = None #self.rc_socket = None
@ -147,7 +89,7 @@ class RemoteController:
self.rc_socket.connect((r.ip, 1234)) # connect to robot self.rc_socket.connect((r.ip, 1234)) # connect to robot
except socket.error: except socket.error:
print("could not connect to socket") print("could not connect to socket")
sys.exit(1) self.rc_socket = None
self.t = time.time() self.t = time.time()
@ -181,10 +123,7 @@ class RemoteController:
# animation # animation
self.fig = plt.figure() self.fig = plt.figure()
self.ax = self.fig.add_subplot(2,2,1) self.ax = self.fig.add_subplot(1,1,1)
self.ax2 = self.fig.add_subplot(2, 2, 2)
self.ax3 = self.fig.add_subplot(2, 2, 4)
self.xdata, self.ydata = [], [] self.xdata, self.ydata = [], []
self.line, = self.ax.plot([],[], color='grey', linestyle=':') self.line, = self.ax.plot([],[], color='grey', linestyle=':')
self.line_sim, = self.ax.plot([], []) self.line_sim, = self.ax.plot([], [])
@ -192,12 +131,6 @@ class RemoteController:
self.dirm, = self.ax.plot([], []) self.dirm, = self.ax.plot([], [])
self.dirs, = self.ax.plot([], []) self.dirs, = self.ax.plot([], [])
self.line_x, = self.ax2.plot([],[])
self.line_y, = self.ax3.plot([], [])
self.track_line_inner, = self.ax.plot([], [])
self.track_line_outer, = self.ax.plot([], [])
self.circles = [] self.circles = []
for o in self.obstacles: for o in self.obstacles:
self.circles.append(patch.Circle((0.0, 0.0), radius=0.1, fill=False, color='red', linestyle='--')) self.circles.append(patch.Circle((0.0, 0.0), radius=0.1, fill=False, color='red', linestyle='--'))
@ -205,20 +138,11 @@ class RemoteController:
for s in self.circles: for s in self.circles:
self.ax.add_artist(s) self.ax.add_artist(s)
self.ax.set_xlabel('x-position') plt.xlabel('x-position')
self.ax.set_ylabel('y-position') plt.ylabel('y-position')
self.ax.grid() plt.grid()
self.ax2.set_xlabel('Zeit t') self.ols = OpenLoopSolver()
self.ax2.set_ylabel('x-position')
self.ax2.grid()
self.ax3.set_xlabel('Zeit t')
self.ax3.set_ylabel('y-position')
self.ax3.grid()
self.mstep = 2
self.ols = OpenLoopSolver(N=20, T=4.0)
self.ols.setup() self.ols.setup()
self.dt = self.ols.T / self.ols.N self.dt = self.ols.T / self.ols.N
@ -227,11 +151,9 @@ class RemoteController:
# integrator # integrator
self.r = scipy.integrate.ode(f_ode) self.r = scipy.integrate.ode(f_ode)
self.omega_max = 5.0 self.omega_max = 5.0
#self.omega_max = 13.32
def ani(self): def ani(self):
print("starting animation")
self.ani = anim.FuncAnimation(self.fig, init_func=self.ani_init, func=self.ani_update, interval=10, blit=True) self.ani = anim.FuncAnimation(self.fig, init_func=self.ani_init, func=self.ani_update, interval=10, blit=True)
plt.ion() plt.ion()
plt.show(block=True) plt.show(block=True)
@ -241,18 +163,7 @@ class RemoteController:
self.ax.set_ylim(-2, 2) self.ax.set_ylim(-2, 2)
self.ax.set_aspect('equal', adjustable='box') self.ax.set_aspect('equal', adjustable='box')
self.ax2.set_ylim(-2, 2) return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol, self.circles[0], self.circles[1],self.circles[2],
self.ax2.set_xlim(0, 10)
self.ax3.set_ylim(-2, 2)
self.ax3.set_xlim(0, 10)
self.track_line_inner.set_data(self.track.inner_poly.exterior.xy)
self.track_line_outer.set_data(self.track.outer_poly.exterior.xy)
return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol,\
self.track_line_inner, self.track_line_outer, self.line_x,self.line_y, self.circles[0], self.circles[1],\
self.circles[2], self.circles[3],
def ani_update(self, frame): def ani_update(self, frame):
#print("plotting") #print("plotting")
@ -276,12 +187,6 @@ class RemoteController:
self.dirm.set_data(np.array([a, a2]), np.array([b, b2])) self.dirm.set_data(np.array([a, a2]), np.array([b, b2]))
n_plot = 300
if len(tm_local) > n_plot:
# plot x and y coordinate
self.line_x.set_data(tm_local[-n_plot:] - (tm_local[-1] - 10), xm_local[-n_plot:,0])
self.line_y.set_data(tm_local[-n_plot:] - (tm_local[-1] - 10), xm_local[-n_plot:, 1])
ts_local = deepcopy(self.ts) ts_local = deepcopy(self.ts)
xs_local = deepcopy(self.xs) xs_local = deepcopy(self.xs)
@ -319,12 +224,9 @@ class RemoteController:
finally: finally:
self.mutex.release() self.mutex.release()
return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol, self.track_line_inner, self.track_line_outer,\ return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol, self.circles[0], self.circles[1],self.circles[2],
self.line_x, self.line_y, self.circles[0], self.circles[1],self.circles[2],self.circles[3],
def measurement_callback(self, data): def measurement_callback(self, data):
#print(data)
# detect robots # detect robots
if data.id in self.robot_ids: if data.id in self.robot_ids:
r = self.robot_ids[data.id] r = self.robot_ids[data.id]
@ -357,26 +259,8 @@ class RemoteController:
obst = (data.x, data.y) obst = (data.x, data.y)
self.obstacles[data.id].pos = obst self.obstacles[data.id].pos = obst
# detect track
if data.id in self.track.inner.keys() or data.id in self.track.outer.keys():
self.track.set_id(data)
def controller(self): def controller(self):
print("starting control") print("starting control")
targets = {}
markers_in = self.track.inner.values()
markers_out = self.track.outer.values()
# find targets:
lamb = 0.3
for i in range(0,4):
p = np.array(markers_in[i]) + lamb * (np.array(markers_out[i]) - np.array(markers_in[i]))
targets[i] = (p[0],p[1], 0.0)
auto_control = False
current_target = 0
while True: while True:
# open loop controller # open loop controller
@ -393,37 +277,21 @@ class RemoteController:
elif event.key == pygame.K_0: elif event.key == pygame.K_0:
self.target = (0.0, 0.0, 0.0) self.target = (0.0, 0.0, 0.0)
elif event.key == pygame.K_1: elif event.key == pygame.K_1:
#self.target = (0.5,0.5, -np.pi/2.0) self.target = (0.5,0.5, -np.pi/2.0)
self.target = targets[0]
elif event.key == pygame.K_2: elif event.key == pygame.K_2:
#self.target = (0.5, -0.5, 0.0) self.target = (0.5, -0.5, 0.0)
self.target = targets[1]
elif event.key == pygame.K_3: elif event.key == pygame.K_3:
#self.target = (-0.5,-0.5, np.pi/2.0) self.target = (-0.5,-0.5, np.pi/2.0)
self.target = targets[2]
elif event.key == pygame.K_4: elif event.key == pygame.K_4:
#self.target = (-0.5,0.5, 0.0) self.target = (-0.5,0.5, 0.0)
self.target = targets[3]
elif event.key == pygame.K_5:
auto_control = not auto_control
if auto_control:
self.target = targets[current_target]
if self.controlling: if self.controlling:
x_pred = self.get_measurement_prediction() x_pred = self.get_measurement_prediction()
if auto_control:
if np.linalg.norm(x_pred[0:2]-np.array(self.target[0:2])) < 0.3:
print("close to target!")
current_target = (current_target + 1) % 4
self.target = targets[current_target]
print("new target = {}".format(current_target))
tmpc_start = time.time() tmpc_start = time.time()
# solve mpc open loop problem # solve mpc open loop problem
res = self.ols.solve(x_pred, self.target, self.obstacles, self.track) res = self.ols.solve(x_pred, self.target, self.obstacles)
us1 = res[0] us1 = res[0]
us2 = res[1] us2 = res[1]
@ -444,24 +312,18 @@ class RemoteController:
time.sleep(self.dt - dt_mpc) time.sleep(self.dt - dt_mpc)
# send controls to the robot # send controls to the robot
for i in range(0, self.mstep): # option to use multistep mpc if len(range) > 1 for i in range(0, 1): # option to use multistep mpc if len(range) > 1
u1 = us1[i] u1 = us1[i]
u2 = us2[i] u2 = us2[i]
if self.rc_socket: if self.rc_socket:
self.rc_socket.send('({},{})\n'.format(u1, u2)) self.rc_socket.send('({},{})\n'.format(u1, u2))
if i < self.mstep:
time.sleep(self.dt)
self.t = time.time() # save time the most recent control was applied self.t = time.time() # save time the most recent control was applied
def get_measurement_prediction(self): def get_measurement_prediction(self):
# get measurement # get measurement
self.mutex.acquire() self.mutex.acquire()
try: try:
window = 3 last_measurement = copy.deepcopy(self.xms[-1])
last_measurement = copy.deepcopy(self.xms[-window:])
#print("last_measurements = {}".format(last_measurement))
#print("mean = {}".format(np.mean(last_measurement, axis=0)))
last_measurement = np.mean(last_measurement, axis=0)
last_time = copy.deepcopy(self.tms[-1]) last_time = copy.deepcopy(self.tms[-1])
finally: finally:
self.mutex.release() self.mutex.release()
@ -487,10 +349,6 @@ def main(args):
screenwidth = 640 screenwidth = 640
pygame.display.set_mode([screenwidth, screenheight]) pygame.display.set_mode([screenwidth, screenheight])
print("waiting until track is completely detected")
while not rc.track.track_complete:
pass
#threading.Thread(target=rc.input_handling).start() #threading.Thread(target=rc.input_handling).start()
threading.Thread(target=rc.controller).start() threading.Thread(target=rc.controller).start()