class Motion():
'''
This class is a high-level motion controller dealing with odometry, navigation, etc.
'''
def __init__(self, IO):
# Multiplier dealing with how the state of the battery affects distance-travelled
# estimates
self.avg_speed = 0.0825 # initial value assumes average / full battery
self.IO = IO
self.sensors = Sensors(self.IO)
self.motors = Motors(self.IO)
self.motors.enable_servo()
self.hall_trig_dist = 0.135675
self.hall_timer = None
self.hall_count = 0
self.last_hall_reading_time = None
self.hall_reading_prev = False
self.angle_time_multiplier = 0.18465
self.servo_engaged = False
def _hall_handler(self):
'''
Called each time step to check what's up with teh hall sensor.
Deals with updating the battery_state estimation which then in turn is used to estimate
distance travelled.
'''
self.sensors.update_readings(type='digital')
hall_reading = self.sensors.get_hall()
time_now = time.time()
# print(hall_reading)
if not hall_reading and self.hall_reading_prev:
self.hall_count += 1
if self.hall_count > 1:
new_avg_speed = self.hall_trig_dist / (time_now - self.hall_timer)
print('new_avg_speed: {}'.format(new_avg_speed))
if (np.abs(new_avg_speed-self.avg_speed) > 0.2 * self.avg_speed):
print("Warning! Average speed estimate changed by more than 20%! (Hall sensor problem?)")
self.avg_speed = new_avg_speed
self.last_hall_reading_time = time_now
self.hall_timer = time_now
print('Hall fall - {}'.format(self.hall_count))
self.hall_reading_prev = hall_reading
def _hall_handler_reset(self):
self.hall_timer = None
self.hall_count = 0
self.sensors.update_readings(type='digital')
self.hall_reading_prev = self.sensors.get_hall()
def move(self, amount, amount_type='distance'):
'''
Moves the robot by the specified amount. By default the amount is distance in meteres.
Set amount_type to 'time' for the amount to be time of motion in seconds, or 'callback'
for amount to be a reference to a callback function to be evaulated periodically (True
result stops the motion).
Negative values move the robot backwards.
Returns estimated distance travelled in meters.
'''
self._hall_handler_reset()
start_time = time.time()
amount_travelled = 0
travel_time_pre_hall = 0
travel_time_since_hall = 0
self.motors.full_on('forward' if amount > 0 else 'backward')
condition = True
while condition:
self._hall_handler()
time_now = time.time()
if self.hall_count == 0:
travel_time_pre_hall = (time_now - start_time)
else:
travel_time_since_hall = (time_now - self.last_hall_reading_time)
amount_travelled = travel_time_pre_hall * self.avg_speed + \
(self.hall_trig_dist * ((self.hall_count - 1) if self.hall_count > 0 else 0) ) + \
travel_time_since_hall * self.avg_speed
if amount_type == 'distance':
condition = amount_travelled <= np.abs(amount)
elif amount_type == 'callback':
condition, reason = amount(self.sensors, amount_travelled)
else:
condition = np.abs(amount) >= (time_now - start_time)
self.motors.stop()
if amount_type == 'callback':
return amount_travelled, reason
else:
return amount_travelled
def turn(self, amount, amount_type='radians'):
'''
Truns the robot by the specified amount. By default the amount is an angle in radians.
Set amount_type to 'degs' for imput in degrees, 'time' for the amount to be time of
rotation in seconds, or 'callback' for amount to be a reference to a callback function
to be evaulated periodically (True result stops the motion). - CALLBACK NOT WORKING YET
Positive values turn the robot clockwise and negative turn it anticlockwise.
Returns estimated angle turned in radians.
'''
start_time = time.time()
amount_travelled = 0
if amount_type == 'radians':
max_time = np.abs(amount) * self.angle_time_multiplier / self.avg_speed
elif amount_type == 'degrees':
max_time = np.abs(amount) * self.angle_time_multiplier / self.avg_speed * np.pi / 180
elif 'time':
max_time = time
self.motors.full_on('right' if amount > 0 else 'left')
condition = True
while condition:
amount_travelled = (time.time() - start_time) * self.avg_speed / self.angle_time_multiplier
if amount_type == 'callback':
condition, reason = amount(self.sensors, amount_travelled)
else:
condition = time.time() - start_time < max_time
self.motors.stop()
if amount_type == 'callback':
return amount_travelled, reason
else:
return amount_travelled
def set_antenna(self, angle, degree_type='radians'):
if not self.servo_engaged:
self.motors.enable_servo()
self.servo_engaged = True
if degree_type == 'radians':
angle *= 180 / np.pi
elif degree_type == 'degrees':
pass
self.motors.set_servo(angle)
