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

version of the problem that works quite nicely with L293D robot and multistep MPC2

Browse Source
This commit is contained in:
Simon Pirkelmann 2019-07-04 14:39:34 +02:00
parent c6582c9e6c
commit d985b830b9
2 changed files with 19 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
remote_control/casadi_opt.py
View File

@ -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.3 maxcontrol = 0.9
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 --------

27
remote_control/position_controller.py
View File

@ -124,14 +124,14 @@ 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(15, '192.168.1.103')]
self.robots = [Robot(14, '192.168.1.102')] #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.25), Obstacle(10, 0.25), Obstacle(13, 0.25)] obst = [Obstacle(12, 0.275), Obstacle(10, 0.275), Obstacle(13, 0.275), Obstacle(14, 0.275)]
self.obstacles = {} self.obstacles = {}
for r in obst: for r in obst:
@ -217,7 +217,8 @@ class RemoteController:
self.ax3.set_ylabel('y-position') self.ax3.set_ylabel('y-position')
self.ax3.grid() self.ax3.grid()
self.ols = OpenLoopSolver() 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
@ -226,6 +227,7 @@ 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):
@ -249,7 +251,8 @@ class RemoteController:
self.track_line_outer.set_data(self.track.outer_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,\ 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.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")
@ -317,7 +320,7 @@ class RemoteController:
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.track_line_inner, self.track_line_outer,\
self.line_x, self.line_y, 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) #print(data)
@ -410,7 +413,7 @@ class RemoteController:
x_pred = self.get_measurement_prediction() x_pred = self.get_measurement_prediction()
if auto_control: if auto_control:
if np.linalg.norm(x_pred[0:2]-np.array(self.target[0:2])) < 0.2: if np.linalg.norm(x_pred[0:2]-np.array(self.target[0:2])) < 0.3:
print("close to target!") print("close to target!")
current_target = (current_target + 1) % 4 current_target = (current_target + 1) % 4
self.target = targets[current_target] self.target = targets[current_target]
@ -440,18 +443,24 @@ 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, 1): # option to use multistep mpc if len(range) > 1 for i in range(0, self.mstep): # 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:
last_measurement = copy.deepcopy(self.xms[-1]) window = 3
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()