class ReachModel(object):
"""Reach model instructs left and right pulse objects.
Pulse objects are orchestrated according to the new reach model.
"""
def __init__(self, ems, gui):
super(ReachModel, self).__init__()
self.verbose = config.getboolean('Reach Control', 'console_stats')
self.brake_channel = config.get("Extra EMS Channels", "brake_active")
self.ems = ems
self.gui = gui
self.left = Pulse(1)
self.right = Pulse(2)
self.penup = Pulse(3)
self.brake = Pulse(4)
self.last_index = 0
self.brake_zone = config.getint('Reach Control', 'brake_zone')
self.fixed_delay = config.getfloat('Reach Control', 'fixed_delay')
pulse_period = config.getfloat('EMS Machine', 'ems_period')
self.reach_repetitions = int(self.fixed_delay / pulse_period)
self.CONTROL_ON = False
self.BRAKE_MODE = False
"""Establishes state transfer from execute_repetions
"""
self.stats = ReachStats()
def create_target(self, canvas):
self.target = Target(canvas)
self.gui.draw_targets(canvas.target_points)
def get_ready_for_next_one(self):
self.last_index = 0
self.stats = ReachStats()
def start_control(self):
"""Waits for the suitable time to start the control strategies.
"""
self.CONTROL_ON = True
while self.CONTROL_ON:
plot_state = self._control_repetions()
if plot_state == 'done':
print('PLOT: ended')
for i in range(50):
self.ems.pulsate(self.penup)
print('ACTION: Pen Up')
break
elif (plot_state == 'no sample' or plot_state == 'no target'
or plot_state == 'nudged'):
time.sleep(self.fixed_delay / 2)
def end_control(self):
self.CONTROL_ON = False
def restart_plot(self):
self.end_control()
time.sleep(self.fixed_delay * 2)
self.last_index = 0
self.stats = ReachStats(self.config)
self.result.refresh_for_next()
def terminate_control(self):
self.end_control()
time.sleep(self.fixed_delay * 2)
self.stats.pause_timer()
duration = self.stats.total_time
print('PLOT: Duration {0:5.1f} seconds.'.format(duration))
# save to file: duration
self.stats.print_timing_analysis()
self.ems.print_timing_analysis()
def handle_penup(self):
self.left.end_boost()
self.right.end_boost()
self.stats.reset_for_pen_up()
def project_speed_to_tilted_axis(self, speed):
if speed[0] == 0:
return (0, 0)
speed_vector_theata = atan(speed[1] / speed[0])
speed_vector_amp = ((speed[0]**2) + (speed[1]**2))**0.5
axis_tilt = radians(-1 * self.target.canvas.major_axis_tilt)
tilted_vector_theata = speed_vector_theata - axis_tilt
projected_x = cos(tilted_vector_theata) * speed_vector_amp
projected_y = sin(tilted_vector_theata) * speed_vector_amp
return (projected_x, projected_y)
def _control_repetions(self):
"""Using speed and location calculate target and distance.
Determine required reach repetions based on parameters and
execute accordingly.
"""
if not self.ems.EMS_ON:
return 'no sample'
current_stats = self.stats.get_last()
if not current_stats:
return 'no sample'
location, speed = current_stats
tilted_speed = self.project_speed_to_tilted_axis(speed)
target_params = self.target.get_target_slope(location, speed)
if target_params == 'done':
return 'done'
elif target_params == 'nudge':
self._execute_nudge()
return 'nudged'
elif target_params:
self.stats.star_timer()
estimate, target, target_slope = target_params
else:
return 'no target'
if len(self.stats.decisions) == 0:
target_slope = 0
if len(self.stats.decisions) == 1:
if abs(target_slope) > 0.3:
if target_slope > 0:
target_slope = 0.3
elif target_slope < 0:
target_slope = -0.3
slope_code = self.left.target_slope(target_slope)
slope_code = self.right.target_slope(target_slope)
self.stats.register_decision(location, estimate, target)
nodes = self.stats.get_targeting()
self.gui.draw_targeting_nodes(nodes)
result = self.stats.analyze_distance_to_target()
brake_repetions = 0
if not result:
pass
elif self.BRAKE_MODE:
result = 'brake'
self.left.end_boost()
self.right.end_boost()
brake_repetions = remap_speed_to_brake(abs(tilted_speed[1]))
self.left.apply_brake()
self.right.apply_brake()
elif result > 0:
if not self.target.canvas.no_boost:
self.right.boost_amp()
self.left.end_boost()
elif result < 0:
if not self.target.canvas.no_boost:
self.left.boost_amp()
self.right.end_boost()
else:
self.left.end_boost()
self.right.end_boost()
# index for decisions to anoto events
index = len(self.stats.events)
samples_between = index - self.last_index
self.last_index = index
if self.verbose:
print(' {0:3d}#'.format(len(self.stats.decisions)), end='')
print(' Slope: {0:6.1f}'.format(target_slope), end='')
indicator = str(int(1000 * 10**-slope_code)).zfill(7)
print(' |{0:3}| '.format(indicator), end='')
print('L: {0:2} '.format(self.left.boost_indicator()), end='')
print('R: {0:2} '.format(self.right.boost_indicator()), end='')
print(' Brake: {0:2}'.format(brake_repetions), end='')
print(' Anoto: {0:2d}'.format(samples_between))
if result == 'brake':
self._execute_repetions(brake_repetions, mode='brake')
else:
self._execute_repetions(self.reach_repetitions, mode='travel')
def _execute_repetions(self, cycles, mode):
"""Pulsates and observes current way to the target.
"""
for cycle in range(cycles):
if not self.ems.EMS_ON:
return
if not mode == 'brake' or not self.brake_channel:
self.ems.pulsate(self.left, self.right)
elif self.brake_channel:
self.ems.pulsate(self.brake)
distances = self.stats.get_distance_to_target()
if distances:
distance, _, _ = distances
if mode == 'brake':
last_speed = self.stats.speed[-1]
tilted_speed = self.project_speed_to_tilted_axis(last_speed)
if (abs(tilted_speed[1]) < 0.1
and not cycles == self.reach_repetitions):
print('BRAKE: ended')
self.BRAKE_MODE = False
return
elif mode == 'travel':
if self.stats.check_target_crossing():
self.BRAKE_MODE = True
print('TRAVEL: ended')
return
if self.BRAKE_MODE:
self.BRAKE_MODE = False
def _execute_nudge(self):
repeat = 9
print('NUDGE: nudging')
self.left.target_slope(20)
for i in range(repeat):
self.ems.pulsate(self.left)
self.right.target_slope(20)
for i in range(repeat):
self.ems.pulsate(self.right)
