def wait_for_position(x, y, th, robot, position_error_margin, th_error_margin):
"""
Wait until the robot reaches the position
:param x: x position to be reached
:param y: y position to be reached
:param robot: robot configuration
:param position_error_margin: error allowed in the position
:param th_error_margin: error allowed in the orientation
"""
[x_odo, y_odo, th_odo] = robot.readOdometry()
print("Waiting for position ", x_odo, y_odo, th_odo, x, y, th)
t_next_period = time.time()
if th is None:
print("TH none")
# None th provided
while position_error_margin < math.sqrt((x_odo - x)**2 +
(y_odo - y)**2):
[x_odo, y_odo, th_odo] = robot.readOdometry()
t_next_period += robot.P
delay_until(t_next_period)
else:
while (position_error_margin < math.sqrt(
(x_odo - x)**2 +
(y_odo - y)**2)) or (th_error_margin < abs(th - th_odo)):
[x_odo, y_odo, th_odo] = robot.readOdometry()
t_next_period += robot.P
delay_until(t_next_period)
# print ([x_odo, y_odo, th_odo])
print([x_odo, y_odo, th_odo])
def build(self, point=0):
if context.balance < self.hub_cost + self.deploy_cost:
return
if self.build_count >= self.max_build:
return
context.balance -= self.hub_cost + self.deploy_cost
self.build_count += 1
points = self.points()
if len(points) == 0:
return
centroids = centroid_locator(points, 0.15, 3)
coords = centroids[point:point + 1]
sector_ids = [coord[0] * self.cols + coord[1] for coord in coords]
self.zero(coords)
new_hubs = ['H{}'.format(sector_id) for sector_id in sector_ids]
api.build_hubs(new_hubs)
count = 0
while True:
status_report = api.status_report()
orders = status_report['orders']
for order in orders:
if order['action'] == 'deliver_hubs':
break
else:
count += 1
if count == 10:
if point == 1:
return
self.build(point=1)
print('Trying again')
print('not found in {}'.format(orders))
continue
break
start_week = order['week']
delay_until(self.start_epoch, start_week + self.hub_weeks,
self.ms_per_week)
api.deploy_hubs(new_hubs, sector_ids)
def wait_for_th(self, th, th_error_margin):
"""
Wait until the robot reaches the position
:param th_error_margin: error allowed in the orientation
"""
[_, _, th_odo] = self.readOdometry()
t_next_period = time.time()
# Repeat while error decrease
last_error = abs(self.normalizeAngle(th - th_odo))
actual_error = last_error
while th_error_margin < actual_error:
last_error = actual_error
while last_error >= actual_error:
[_, _, th_odo] = self.readOdometry()
last_error = actual_error
actual_error = abs(self.normalizeAngle(th - th_odo))
t_next_period += self.P
delay_until(t_next_period)
def wait_for_position(self,
x,
y,
position_error_margin,
minimize_error=True):
"""
Wait until the robot reaches the position
:param x: x position to be reached
:param y: y position to be reached
:param position_error_margin: error allowed in the position
:param minimize_error: if true, the loop will keep control until the last error is minor to actual error
"""
[x_odo, y_odo, _] = self.readOdometry()
t_next_period = time.time()
# Repeat while error decrease
last_error = math.sqrt((x_odo - x)**2 + (y_odo - y)**2)
actual_error = last_error
if minimize_error:
# Minimize error
while position_error_margin < actual_error:
last_error = actual_error
while last_error >= actual_error:
[x_odo, y_odo, _] = self.readOdometry()
last_error = actual_error
actual_error = math.sqrt((x_odo - x)**2 + (y_odo - y)**2)
t_next_period += self.P
delay_until(t_next_period)
else:
# Stop when be in error margin
while position_error_margin < math.sqrt((x_odo - x)**2 +
(y_odo - y)**2):
[x_odo, y_odo, _] = self.readOdometry()
t_next_period += self.P
delay_until(t_next_period)
print("He llegado a : ", x_odo, y_odo, " y busco: ", x, y)
def updateOdometry(self, x_odo, y_odo, th_odo, finished):
"""
Update odometry every period
:param x_odo: value where x coordinate must be stored
:param y_odo: value where y coordinate must be stored
:param th_odo: value where angle must be stored
:param finished: if finish is true, odometry must stop updating
"""
# current processor time in a floating point value, in seconds
t_next_period = time.time()
while not finished.value:
d_t = self.P
[v, w] = self.readSpeed()
x = x_odo.value
y = y_odo.value
th = th_odo.value
if w == 0:
# Straight movement
x = x + d_t * v * math.cos(th)
y = y + d_t * v * math.sin(th)
else:
# Curved movement
x = x + (v / w) * (math.sin(th + w * d_t) - math.sin(th))
y = y - (v / w) * (math.cos(th + w * d_t) - math.cos(th))
# Check if use sensors
self.lock_odometry.acquire()
is_spinning = self.is_spinning.value
enable_gyro_sensors = self.enable_gyro_sensors.value
enable_proximity_sensor = self.enable_proximity_sensor.value
self.lock_odometry.release()
# Get sensors data
gyro_1, gyro_2, proximity = self.readSensors(
read_proximity=enable_proximity_sensor,
read_gyro=enable_gyro_sensors)
# Obtain precise th
if is_spinning and enable_gyro_sensors:
# Only if it is turning on read gyro sensors
# Sensor 1
actual_value_gyro_1 = -(gyro_1 - self.gyro_1_offset
) * self.gyro_1_correction_factor * d_t
# Sensor 2
actual_value_gyro_2 = -(gyro_2 - self.gyro_2_offset
) * self.gyro_2_correction_factor * d_t
w = (w + actual_value_gyro_1 + actual_value_gyro_2) / 3.0
else:
self.history_gyro_1.append(self.gyro_1_offset)
self.history_gyro_2.append(self.gyro_2_offset)
# Update th
th = th + d_t * w
# Update odometry
self.lock_odometry.acquire()
if not self.odometry_reseted.value:
x_odo.value = x
y_odo.value = y
th_odo.value = self.normalizeAngle(th)
else:
# In case odometry is reseted, jump this updating period
self.odometry_reseted.value = False
self.proximity.value = proximity
self.lock_odometry.release()
# Periodic task
t_next_period += self.P
delay_until(t_next_period)
sys.stdout.write("Stopping odometry ... X= %d, \
Y= %d, th= %d \n" % (x_odo.value, y_odo.value, th_odo.value))