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

working version with two robots

simple_control
Simon Pirkelmann 2020-09-09 18:12:12 +02:00
parent 280aee75ee
commit c98760a2c7
1 changed files with 48 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

92
remote_control/roborally.py
View File

@ -61,6 +61,26 @@ class Robot:
self.euler = None
self.ip = ip
self.socket = socket.socket()
# variables for measurements
self.tms_0 = None
self.xm_0 = None
self.tms = None
self.xms = None
# currently active control
self.u1 = 0.0
self.u2 = 0.0
def connect(self):
# connect to robot
try:
self.socket.connect((self.ip, 1234)) # connect to robot
except socket.error:
print("could not connect to socket")
sys.exit(1)
def f_ode(t, x, u):
# dynamical model of the two-wheeled robot
@ -89,9 +109,9 @@ class RemoteController:
self.anim_stopped = False
#self.robots = [Robot(14, '192.168.1.103')]
self.robots = [Robot(11, '192.168.1.101'), Robot(14, '192.168.1.103')]
#self.robots = [Robot(15, '192.168.1.102')]
self.robots = [Robot(id, ip)]
#self.robots = [Robot(id, ip)]
self.robot_ids = {}
for r in self.robots:
@ -99,20 +119,13 @@ class RemoteController:
self.valid_cmds = ['forward', 'backward', 'turn left', 'turn right']
# connect to robot
self.rc_socket = socket.socket()
#self.rc_socket = None
try:
for r in self.robots:
self.rc_socket.connect((r.ip, 1234)) # connect to robot
except socket.error:
print("could not connect to socket")
sys.exit(1)
# socket for movement commands
self.comm_socket = socket.socket()
self.comm_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
for robot in self.robots:
robot.connect()
#self.comm_socket.bind((socket.gethostname(), 1337))
self.comm_socket.bind(('', 1337))
self.comm_socket.listen(5)
@ -124,11 +137,6 @@ class RemoteController:
self.ts = np.array([])
self.xs = []
# variables for measurements
self.tms_0 = None
self.xm_0 = None
self.tms = None
self.xms = None
# variable for mpc open loop
self.ol_x = None
@ -142,10 +150,6 @@ class RemoteController:
# pid parameters
self.controlling = False
# currently active control
self.u1 = 0.0
self.u2 = 0.0
# animation
self.fig = plt.figure()
self.ax = self.fig.add_subplot(2,2,1)
@ -187,7 +191,7 @@ class RemoteController:
# integrator
self.r = scipy.integrate.ode(f_ode)
self.omega_max = 5.0
self.control_scaling = 0.2
self.control_scaling = 0.3
#self.omega_max = 13.32
@ -289,21 +293,21 @@ class RemoteController:
# save measured position and angle for plotting
measurement = np.array([r.pos[0], r.pos[1], r.euler])
if self.tms_0 is None:
self.tms_0 = time.time()
self.xm_0 = measurement
if r.tms_0 is None:
r.tms_0 = time.time()
r.xm_0 = measurement
self.mutex.acquire()
try:
self.tms = np.array([0.0])
self.xms = measurement
r.tms = np.array([0.0])
r.xms = measurement
finally:
self.mutex.release()
else:
self.mutex.acquire()
try:
self.tms = np.vstack((self.tms, time.time() - self.tms_0))
self.xms = np.vstack((self.xms, measurement))
r.tms = np.vstack((r.tms, time.time() - r.tms_0))
r.xms = np.vstack((r.xms, measurement))
finally:
self.mutex.release()
@ -388,8 +392,8 @@ class RemoteController:
self.t = time.time()
elif event.key == pygame.K_DOWN:
self.controlling = False
if self.rc_socket:
self.rc_socket.send('(0.0,0.0)\n')
if self.robot_ids[robot_id].socket:
self.robot_ids[robot_id].socket.send('(0.0,0.0)\n')
elif event.key == pygame.K_0:
self.target = np.array([0,0,0])
elif event.key == pygame.K_PLUS:
@ -403,20 +407,20 @@ class RemoteController:
elif event.key == pygame.K_ESCAPE:
print("quit!")
self.controlling = False
if self.rc_socket:
self.rc_socket.send('(0.0,0.0)\n')
if self.robot_ids[robot_id].socket:
self.robot_ids[robot_id].socket.send('(0.0,0.0)\n')
self.anim_stopped = True
return
elif event.key == pygame.QUIT:
print("quit!")
self.controlling = False
if self.rc_socket:
self.rc_socket.send('(0.0,0.0)\n')
if self.robot_ids[robot_id].socket:
self.robot_ids[robot_id].socket.send('(0.0,0.0)\n')
self.anim_stopped = True
return
if self.mpc:
x_pred = self.get_measurement_prediction()
x_pred = self.get_measurement_prediction(robot_id)
tmpc_start = time.time()
@ -424,9 +428,9 @@ class RemoteController:
angles_unwrapped = np.unwrap([x_pred[2], self.target[2]]) # unwrap angle to avoid jump in data
error_ang = np.abs(angles_unwrapped[0] - angles_unwrapped[1])
#print("error pos = ", error_pos)
print(" error_ang = {}, target = {}, angle = {}".format(error_ang, self.target[2], x_pred[2]))
print("error_pos = {}, error_ang = {}".format(error_pos, error_ang))
if error_pos > 0.1 or error_ang > 0.35:
if error_pos > 0.075 or error_ang > 0.35:
# solve mpc open loop problem
res = self.ols.solve(x_pred, self.target)
@ -461,28 +465,28 @@ class RemoteController:
for i in range(0, self.mstep): # option to use multistep mpc if len(range) > 1
u1 = us1[i]
u2 = us2[i]
if self.rc_socket:
self.rc_socket.send('({},{})\n'.format(u1, u2))
if self.robot_ids[robot_id].socket:
self.robot_ids[robot_id].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
def get_measurement_prediction(self):
def get_measurement_prediction(self, robot_id):
# get measurement
self.mutex.acquire()
try:
window = 3
last_measurement = copy.deepcopy(self.xms[-window:])
last_measurement = copy.deepcopy(self.robot_ids[robot_id].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.robot_ids[robot_id].tms[-1])
finally:
self.mutex.release()
# prediction of state at time the mpc will terminate
self.r.set_f_params(np.array([self.u1 * self.omega_max, self.u2 * self.omega_max]))
self.r.set_f_params(np.array([self.robot_ids[robot_id].u1 * self.omega_max, self.robot_ids[robot_id].u2 * self.omega_max]))
self.r.set_initial_value(last_measurement, last_time)