def modify(self, left, right):
handle_time_stamp = self._handle_time_stamp
if handle_time_stamp is None or time.time() - handle_time_stamp > 0.8:
scan = None
else:
scan = self._scan
_time_stamp = time.time()
_delta_time = _time_stamp - self._time_stamp
self._time_stamp = _time_stamp
if scan is not None:
current_angle = logic.get_angle(left, right, self._robo_width)
current_speed = logic.get_speed(left, right)
min_distance, min_distance_angle = logic.get_min_distance(scan, current_angle,
self._scanner_dist_offset, self._angle_range)
max_distance, max_distance_angle = logic.get_max_distance(scan, current_angle,
self._scanner_dist_offset, self._angle_range)
# print 'rodeo-swap : min: %d, %d; max: %d, %d' % (min_distance, math.degrees(min_distance_angle),
# max_distance, math.degrees(max_distance_angle))
if min_distance is not None:
soft_limit = logic.get_soft_limit(current_speed, self._max_speed,
self._soft_limit, self._hard_limit, self._alpha)
if min_distance < soft_limit:
if max_distance_angle > current_angle or min_distance_angle < current_angle:
if left > 0:
max_speed = logic.get_max_speed(min_distance, soft_limit,
self._hard_limit, self._max_rotating_speed)
left = left if left < max_speed else max_speed
right = -left # FIXME(paoolo)
else:
if right > 0:
_t = left
left = right
right = _t
elif max_distance_angle < current_angle or min_distance_angle > current_angle:
if right > 0:
max_speed = logic.get_max_speed(min_distance, soft_limit,
self._hard_limit, self._max_rotating_speed)
right = right if right < max_speed else max_speed
left = -right # FIXME(paoolo)
else:
if left > 0:
_t = right
right = left
left = _t
elif min_distance < soft_limit * self._beta:
left = -left
right = -right
else:
print 'rodeo-swap: no scan!'
left, right = 0.0, 0.0
return left, right
def modify(self, left, right):
if abs(self._x - self._target_x) < self._target_radius \
and abs(self._y - self._target_y) < self._target_radius:
return 0.0, 0.0
if self.roboclaw is None:
return left, right
_speed = self.roboclaw.get_current_motors_speed()
_speed_left, speed_right = _speed.get_front_left_speed(), _speed.get_front_right_speed()
if abs(_speed_left - left) > 500.0 or abs(speed_right - right) > 500.0:
return left, right
_time_stamp = time.time()
_delta_time = _time_stamp - self._time_stamp
self._time_stamp = _time_stamp
self._x, self._y, self._robo_angle = self._compute_new_relative_position(_speed_left, speed_right, _delta_time)
_target_angle = math.atan2(self._target_y - self._y, self._target_x - self._x)
_drive_angle = _target_angle - self._robo_angle
_drive_angle = DriveToPoint._normalize_angle(_drive_angle)
if not self._active:
self._active = (abs(_drive_angle) > 2.0 * math.pi / 3.0)
else:
self._active = not (abs(_drive_angle) < 0.03490658503988659)
print 'drive_to_point: %s, %f, %f, %f, %f, %f, %f' % (str(self._active),
math.degrees(_target_angle),
math.degrees(self._robo_angle),
math.degrees(_drive_angle),
_drive_angle / math.pi,
self._x, self._y)
if not self._active:
return left, right
else:
_drive_speed = logic.get_speed(left, right)
return _drive_speed - self._alpha * _drive_angle / math.pi * _drive_speed, \
_drive_speed + self._alpha * _drive_angle / math.pi * _drive_speed
