class Robot(object): '''This is the logic/execution module for the pykhepera robot. It handles the high level functions such as reloading the robot, controlling the robot, etc. ''' def __init__(self, num_particles=100, plotting=False): super(Robot, self).__init__() self.found_food = False self.carrying = 0 self.data = Data() self.plotting = plotting self.r = pykhepera.PyKhepera() self.axel_l = 53.0 # self.axis length in mm self.data.clear() self.data.pose = utils.Particle(utils.home_pose.x, utils.home_pose.y, utils.home_pose.theta, 0.5) self.ppose = utils.home_pose self.particle_filter = ParticleFilter(num_particles, self.data.pose, self.data) self.data.load_calibration() self.speed_command = (0,0) if self.plotting: self.init_plotting() def init_plotting(self): plt.ion() self.fig = plt.figure(figsize=(16, 12)) self.ax = self.fig.add_subplot(1, 1, 1) self.ax.axis([0,1469, 0, 962]) self.line, = self.ax.plot(self.data.x_positions, self.data.y_positions) self.p_lines, = self.ax.plot(self.particle_filter.get_x(), self.particle_filter.get_y(), 'r.') self.ppose_lines, = self.ax.plot(self.ppose.x, self.ppose.y, 'go') self.im = np.flipud(misc.imread('arena.bmp')) plt.imshow(self.im, origin='lower') self.plotting_thread = threading.Thread(target=self.update_plot) self.plotting_thread.daemon = True self.plotting_thread.start() def update_data(self): self.data.sensor_values = self.r.read_sensor_values() wheel_speeds = self.r.read_wheel_speeds() self.data.wheel_speeds = wheel_speeds self.data.wheel_values = self.r.read_wheel_values() def calibrate_min(self): self.r.reset_wheel_counters() self.update_data() mins = self.data.sensor_values maxs = mins self.r.turn((5, -5)) print self.data.wheel_values while self.data.wheel_values[0] < 2000: self.update_data() aux = self.data.sensor_values for i, val in enumerate(aux): if val > mins[i]: mins[i] = val else: maxs[i] = val self.r.turn((0, 0)) m = 0 for val in mins: m += val m = m/8 maxs_avg = sum((val in maxs))/len(maxs) self.data.thresholds['max_ir_reading'] = m self.data.thresholds['wall_max'] = m self.data.thresholds['wall_min'] = maxs_avg if m: print 'calibration succesful: ', mins fout = open('data_calibration.data', 'a') fout.write('%d:%s' % (m, ''.join(str(mins)))) fout.write('\n') fout.close() def largest_reading(self, sensor_i, duration=1, sample_rate=.5): self.update_data() max_reading = self.data.sensor_values[sensor_i] for i in range(int(duration/sample_rate)): self.update_data() if self.data.sensor_values[sensor_i] > max_reading: max_reading = self.data.sensor_values[sensor_i] print 'reading: ', self.data.sensor_values[sensor_i] time.sleep(sample_rate) print 'max_reading for %d: %d' % (sensor_i, max_reading) return max_reading def calibrate_distances(self, closest=2, furthest=4): readings = [] for i in range(9): aux = [] readings.append(aux) self.update_data() par = -1 for a in range(9): self.r.set_values('G', [0, 0]) readings[6].append(self.largest_reading(6)) readings[7].append(self.largest_reading(7)) self.r.rotate(par * np.pi/2) time.sleep(1) readings[0].append(self.largest_reading(0)) self.r.rotate(par * np.pi/4) time.sleep(0.3) readings[1].append(self.largest_reading(1)) self.r.rotate(par * np.pi/4) time.sleep(0.3) readings[2].append(self.largest_reading(2)) readings[3].append(self.largest_reading(3)) self.r.rotate(par * np.pi/4) time.sleep(0.3) readings[4].append(self.largest_reading(4)) self.r.rotate(par * np.pi/4) time.sleep(0.3) readings[5].append(self.largest_reading(5)) self.r.set_values('C', [0, 0]) time.sleep(3) self.r.travel(utils.to_wu(10)) time.sleep(1) for i in range(8): sensor = 'sensor'+str(i) self.data.thresholds[sensor] = readings[i] # print self.data.thresholds # avg_max = sum(r[closest] for r in self.data.distance_thresholds)/len(self.data.distance_thresholds) # avg_min = sum(r[furthest] for r in self.data.distance_thresholds)/len(self.data.distance_thresholds) # self.data.thresholds['max_ir_reading'] = avg_max # self.data.thresholds['wall_min'] = avg_min self.data.save_calibration() def update_plot(self): while self.plotting: self.line.set_xdata(self.data.x_positions) self.line.set_ydata(self.data.y_positions) self.p_lines.set_xdata(self.particle_filter.get_x()) self.p_lines.set_ydata(self.particle_filter.get_y()) self.ppose_lines.set_xdata(self.ppose.x) self.ppose_lines.set_ydata(self.ppose.y) time.sleep(.1) def will_collide(self): for val in self.data.sensor_values[1:5]: if val > self.data.thresholds['max_ir_reading']: return True def led_state(self, number): '''converts a number to binary and lights LED accordingly''' if number > 3: return binary = bin(number)[2:] if len(binary) == 1: binary = '0' + binary for i, bit in enumerate(binary): self.r.led(i, bit) def update(self, dt): self.update_data() self.data.pose = utils.update_pose(self.data.pose, self.data.wheel_speeds, dt) self.data.x_positions.append(self.data.pose.x) self.data.y_positions.append(self.data.pose.y) self.data.theta = self.data.pose.theta self.particle_filter.update(dt) self.ppose = self.particle_filter.most_likely() if self.plotting: self.fig.canvas.draw() def check_food(self): if len(self.data.sensor_values) == 0: return for val in self.data.sensor_values: if val < 300: return else: self.found_food = True self.carrying += 1 def flash_leds(self): for i in range(4): self.led_state(3) self.led_state(0) def run(self): obj_avoid = behaviors.ObjAvoid(self.data) # wall_follow = behaviors.WallFollow(self.data) # go_home = behaviors.GoHome(self.data) last_time = time.time() time_up = False self.update_data() try: while True: current_time = time.time() # if (not time_up and current_time - start_time > 5): # time_up = True # if self.r.state is not 0: # self.r.state = 3 dt = (current_time - last_time) last_time = time.time() self.check_food() if self.found_food: print "fooooooood!!" self.found_food = False self.r.stop() self.flash_leds() self.update(dt) speed = (0, 0) vals = self.data.sensor_values if self.r.state is 0: self.led_state(0) if self.will_collide(): speed = (0, 0) self.r.state = 1 else: if self.carrying: speed = (5, 5) else: speed = (5, 5) if self.r.state is 1: self.led_state(1) speed = obj_avoid.step() # if speed == (5, 5): # if time_up: # self.r.state = 3 # else: # self.r.state = 2 # wall_follow.prev_vals = vals # speed = (0, 0) elif self.r.state is 2: self.led_state(2) if self.will_collide(): self.r.state = 1 continue if (vals[0] < self.data.thresholds['wall_min'] and vals[5] < self.data.thresholds['wall_min']): #"No wall :( " self.r.state = 0 continue speed = wall_follow.step() #print "Following :)" elif self.r.state is 3: print 'going home' self.led_state(3) self.r.stop() suggestion = go_home.step() rotation = suggestion['rotation'] print 'wheel values: ', self.data.wheel_values print 'rotation: ', rotation self.r.rotate(rotation) time.sleep(2) self.r.state = 0 if speed: self.r.turn(speed) self.speed_command = speed except KeyboardInterrupt: print 'killing and cleaning up' self.r.purge_buffer() self.led_state(0) self.plotting = False self.plotting_thread.join() self.r.stop() self.r.kill() self.particle_filter.tear_down()