def end_mode(self):
ModeProgram.end_mode(self)
if self.mode in ['fwd-1', 'ccw-c', 'cw-c']:
return
# Keep track of the current x-position.
d = self.mode_direction()
dist = self.time_to_dist(self.mode_time())
if d == 'fwd':
if self.heading == 'out':
self.x_pos += dist
elif self.heading == 'in':
self.x_pos -= dist
elif d == 'bwd':
if self.heading == 'out':
self.x_pos -= dist
elif self.heading == 'in':
self.x_pos += dist
elif d == 'ccw':
if self.heading == 'up':
self.heading = 'out'
elif self.heading == 'in':
self.heading = 'up'
elif d == 'cw':
if self.heading == 'up':
self.heading = 'in'
elif self.heading == 'out':
self.heading = 'up'
def end_mode(self):
ModeProgram.end_mode(self)
if self.mode in ['fwd-1', 'ccw-c', 'cw-c']:
return
# Keep track of the current x-position.
d = self.mode_direction()
dist = self.time_to_dist(self.mode_time())
if d == 'fwd':
if self.heading == 'out':
self.x_pos += dist
elif self.heading == 'in':
self.x_pos -= dist
elif d == 'bwd':
if self.heading == 'out':
self.x_pos -= dist
elif self.heading == 'in':
self.x_pos += dist
elif d == 'ccw':
if self.heading == 'up':
self.heading = 'out'
elif self.heading == 'in':
self.heading = 'up'
elif d == 'cw':
if self.heading == 'up':
self.heading = 'in'
elif self.heading == 'out':
self.heading = 'up'
def reset(self):
ModeProgram.reset(self)
self.x_pos = 0
self.heading = 'up'
self.around_mult_f = 1
self.around_mult = 1
self.first_obstacle_reading = 0
self.side = 'front'
def reset(self):
ModeProgram.reset(self)
self.x_pos = 0
self.heading = 'up'
self.around_mult_f = 1
self.around_mult = 1
self.first_obstacle_reading = 0
self.side = 'front'
def move(self):
"""Makes Myro calls to move the robot according to the current mode.
Called when the mode is begun and whenever the program is resumed."""
ModeProgram.move(self)
if self.mode == 0 or self.mode == 'halt':
myro.stop()
if self.mode == 'drive':
myro.forward(self.speed)
if self.mode == 'rotate':
myro.rotate(self.rot_dir * self.speed)
def move(self):
"""Makes Myro calls to move the robot according to the current mode.
Called when the mode is begun and whenever the program is resumed."""
ModeProgram.move(self)
if self.mode == 0 or self.mode == 'halt':
myro.stop()
if self.mode == 'drive':
myro.forward(self.speed)
if self.mode == 'rotate':
myro.rotate(self.rot_dir * self.speed)
def reset(self):
ModeProgram.reset(self)
self.points = None # path the the robot draws
self.index = 0 # index of point robot is going towards
self.heading = math.pi / 2 # the current heading, in standard position
self.rot_dir = 1 # 1 for counterclockwise, -1 for clockwise
self.go_for = 0 # the time duration of the robot's current action
# These two are only needed because the status method needs to access
# them after they have been set by the time setting methods.
self.delta_angle = 0
self.delta_pos = 0
def move(self):
ModeProgram.move(self)
direction = self.mode_direction()
if direction == 'fwd':
myro.forward(self.speed)
if direction == 'bwd':
myro.backward(self.speed)
if direction == 'ccw':
myro.rotate(self.around_mult * self.speed)
if direction == 'cw':
myro.rotate(self.around_mult * -self.speed)
def reset(self):
ModeProgram.reset(self)
self.points = None # path the the robot draws
self.index = 0 # index of point robot is going towards
self.heading = math.pi / 2 # the current heading, in standard position
self.rot_dir = 1 # 1 for counterclockwise, -1 for clockwise
self.go_for = 0 # the time duration of the robot's current action
# These two are only needed because the status method needs to access
# them after they have been set by the time setting methods.
self.delta_angle = 0
self.delta_pos = 0
def move(self):
ModeProgram.move(self)
direction = self.mode_direction()
if direction == 'fwd':
myro.forward(self.speed)
if direction == 'bwd':
myro.backward(self.speed)
if direction == 'ccw':
myro.rotate(self.around_mult * self.speed)
if direction == 'cw':
myro.rotate(self.around_mult * -self.speed)
def __call__(self, command):
p_status = ModeProgram.__call__(self, command)
if p_status:
return p_status
if command.startswith(POINTS_PREFIX):
json_str = command[len(POINTS_PREFIX):]
self.new_points = self.transform_points(json.loads(json_str))
return "received {} points".format(str(len(self.new_points)))
if command == 'short:trace':
if self.mode == 0:
return "0 {}".format(self.heading)
if self.mode == 'halt':
return "{} {}".format(len(self.points) - 1, self.heading)
t = self.mode_time()
T = self.go_for
i = self.index - 1
delta_i = 1
theta = self.heading
delta_theta = 0
if self.mode == 'rotate':
delta_i = 0
delta_theta = self.delta_angle
theta -= delta_theta
vals = [t, T, i, delta_i, theta, delta_theta]
return ' '.join(map(str, vals))
def __call__(self, command):
p_status = ModeProgram.__call__(self, command)
if p_status:
return p_status
if command.startswith(POINTS_PREFIX):
json_str = command[len(POINTS_PREFIX):]
self.new_points = self.transform_points(json.loads(json_str))
return "received {} points".format(str(len(self.new_points)))
if command == 'short:trace':
if self.mode == 0:
return "0 {}".format(self.heading)
if self.mode == 'halt':
return "{} {}".format(len(self.points)-1, self.heading)
t = self.mode_time()
T = self.go_for
i = self.index - 1
delta_i = 1
theta = self.heading
delta_theta = 0
if self.mode == 'rotate':
delta_i = 0
delta_theta = self.delta_angle
theta -= delta_theta
vals = [t, T, i, delta_i, theta, delta_theta]
return ' '.join(map(str, vals));
def __call__(self, command):
p_status = ModeProgram.__call__(self, command)
if p_status:
return p_status
if command == 'short:att':
if self.running:
t = self.mode_time()
s = self.speed
angle = self.params['calib_angle']
return str(t * s / angle)
else:
return "program not running"
def __call__(self, command):
p_status = ModeProgram.__call__(self, command)
if p_status:
return p_status
if command == 'short:att':
if self.running:
t = self.mode_time()
s = self.speed
angle = self.params['calib_angle']
return str(t * s / angle)
else:
return "program not running"
def stop(self):
ModeProgram.stop(self)
self.running = False
def __init__(self):
# self.new_points is the list of points that will be used next.
# It persists across resets.
self.new_points = []
ModeProgram.__init__(self, 0)
self.add_params(PARAM_DEFAULTS, PARAM_CODES)
def loop(self):
ModeProgram.loop(self)
if self.is_mode_done():
self.next_mode()
return self.status()
def __init__(self):
# self.new_points is the list of points that will be used next.
# It persists across resets.
self.new_points = []
ModeProgram.__init__(self, 0)
self.add_params(PARAM_DEFAULTS, PARAM_CODES)
def start(self):
ModeProgram.start(self)
self.running = True
def stop(self):
ModeProgram.stop(self)
self.running = False
def __init__(self):
ModeProgram.__init__(self, 0)
self.add_params(PARAM_DEFAULTS, PARAM_CODES)
def loop(self):
ModeProgram.loop(self)
if self.mode == 0:
return self.goto('fwd-1')
if self.mode == 'fwd-1':
d = obstacle_average()
if d > self.params['obstacle_thresh']:
self.first_obstacle_reading = d
return self.goto('ccw-c')
if self.mode == 'ccw-c':
if self.has_rotated(self.params['compare_rotation']):
myro.stop()
d = obstacle_average()
if d < self.first_obstacle_reading:
self.around_mult_f = 1
else:
self.around_mult_f = -1
return self.goto('cw-c')
if self.mode == 'cw-c':
if self.has_rotated(self.params['compare_rotation']):
self.around_mult = self.around_mult_f
return self.goto('ccw-1')
if self.mode == 'ccw-1':
if self.at_right_angle():
return self.goto('fwd-2')
if self.mode == 'fwd-2':
if self.has_travelled(self.params['check_dist']):
return self.goto('cw-1')
if self.mode == 'cw-1':
if self.at_right_angle():
myro.stop()
if obstacle_average() > self.params['obstacle_thresh']:
return self.goto('ccw-1')
else:
return self.goto('ccw-2')
if self.mode == 'ccw-2':
if self.at_right_angle():
return self.goto('fwd-3')
if self.mode == 'fwd-3':
if self.has_travelled(self.params['overshoot_front']):
return self.goto('cw-2')
if self.mode == 'cw-2':
if self.at_right_angle():
if self.side == 'front':
return self.goto('fwd-4')
else:
return self.goto('fwd-5')
if self.mode == 'fwd-4':
if self.has_travelled(self.params['overshoot_side']):
self.side = 'side'
return self.goto('cw-1')
if obstacle_average() > self.params['obstacle_thresh']:
myro.stop()
return self.goto('ccw-1')
if self.mode == 'fwd-5':
if self.has_travelled(self.x_pos * self.params['return_factor']):
return self.goto('ccw-3')
if obstacle_average() > self.params['obstacle_thresh']:
myro.stop()
return self.goto('ccw-1')
if self.mode == 'ccw-3':
if self.at_right_angle():
self.reset()
self.start()
return "restarting program"
def start(self):
ModeProgram.start(self)
self.running = True
def __init__(self):
ModeProgram.__init__(self, 0)
self.add_params(PARAM_DEFAULTS, PARAM_CODES)
def loop(self):
ModeProgram.loop(self)
if self.is_mode_done():
self.next_mode()
return self.status()
def loop(self):
ModeProgram.loop(self)
if self.mode == 0:
return self.goto('fwd-1')
if self.mode == 'fwd-1':
d = obstacle_average()
if d > self.params['obstacle_thresh']:
self.first_obstacle_reading = d
return self.goto('ccw-c')
if self.mode == 'ccw-c':
if self.has_rotated(self.params['compare_rotation']):
myro.stop()
d = obstacle_average()
if d < self.first_obstacle_reading:
self.around_mult_f = 1
else:
self.around_mult_f = -1
return self.goto('cw-c')
if self.mode == 'cw-c':
if self.has_rotated(self.params['compare_rotation']):
self.around_mult = self.around_mult_f
return self.goto('ccw-1')
if self.mode == 'ccw-1':
if self.at_right_angle():
return self.goto('fwd-2')
if self.mode == 'fwd-2':
if self.has_travelled(self.params['check_dist']):
return self.goto('cw-1')
if self.mode == 'cw-1':
if self.at_right_angle():
myro.stop()
if obstacle_average() > self.params['obstacle_thresh']:
return self.goto('ccw-1')
else:
return self.goto('ccw-2')
if self.mode == 'ccw-2':
if self.at_right_angle():
return self.goto('fwd-3')
if self.mode == 'fwd-3':
if self.has_travelled(self.params['overshoot_front']):
return self.goto('cw-2')
if self.mode == 'cw-2':
if self.at_right_angle():
if self.side == 'front':
return self.goto('fwd-4')
else:
return self.goto('fwd-5')
if self.mode == 'fwd-4':
if self.has_travelled(self.params['overshoot_side']):
self.side = 'side'
return self.goto('cw-1')
if obstacle_average() > self.params['obstacle_thresh']:
myro.stop()
return self.goto('ccw-1')
if self.mode == 'fwd-5':
if self.has_travelled(self.x_pos * self.params['return_factor']):
return self.goto('ccw-3')
if obstacle_average() > self.params['obstacle_thresh']:
myro.stop()
return self.goto('ccw-1')
if self.mode == 'ccw-3':
if self.at_right_angle():
self.reset()
self.start()
return "restarting program"
