def fwd_amt(distance): global WHEELRADIUS, speed_left, speed_right, particle_counter, circumference BrickPiUpdateValues() offset_1 = BrickPi.Encoder[motor1] offset_2 = BrickPi.Encoder[motor2] speed_left = 200 speed_right = 204 print "- Going forward ", distance, " cm" no_rotations = distance / circumference degrees = no_rotations * 720 BrickPi.MotorSpeed[motor1] = speed_left BrickPi.MotorSpeed[motor2] = speed_right throttle_index = 0 print "deg", degrees, no_rotations previous_offset = offset_1 while(BrickPi.Encoder[motor1] - offset_1 < degrees and BrickPi.Encoder[motor2] - offset_2 < degrees): # running while loop for no_seconds seconds BrickPiUpdateValues() # Ask BrickPi to update values for sensors/motors adjustValues(degrees, offset_1, offset_2) throttle_index += 1 delta_distance = (circumference * (BrickPi.Encoder[motor1] - previous_offset) / 720) if delta_distance > 10: particles.update_forward(delta_distance) print "Moved", delta_distance, "cm - updating particle map" previous_offset = BrickPi.Encoder[motor1] particles.draw() throttle_index = 0 time.sleep(.001) # sleep for 100 ms
def checkToDraw(offset): global particle_counter, circumference if particle_counter >= 250: distance = circumference * (BrickPi.Encoder[motor1] - offset) / 720 particles.update_forward(distance) particles.draw() particle_counter = 0
def turnParticleDraw(turned): print turned global turn_counter turn_counter += 1 if turn_counter >= 250: particles.update_rotate(turned) particles.draw() turn_counter = 0
def turnParticleDraw(turned): print turned global turn_counter turn_counter += 1 if turn_counter >= 250: particles.update_rotate(turned) particles.update_probability(get_sonar_distance()) particles.draw() turn_counter = 0
def drawstuff(self): if self.mywalker!=None: display.lookat((self.mywalker.x+self.mywalker.gmx)/2, (self.mywalker.y+self.mywalker.gmy)/2) self.drawradar() display.clear() display.applycam() terrain.draw(display.camx,display.camy) for b in self.walkers: b.draw() particles.draw() self.menu.drawLog() pygame.display.flip()
def navigateToWaypoint(x, y): (currentX, currentY, currentAngle) = particles.estimate_location() print "Navigating from (",currentX,", ",currentY,") -> (",x,", ",y,")" dx = x - currentX dy = y - currentY distance = math.sqrt(dx**2 + dy**2) theta_portion = math.fabs(math.degrees(math.atan(dy / dx))) print "Current Angle: ", currentAngle target_theta = 0 if dx > 0 and dy > 0: #target_theta = 360 - theta_portion target_theta = theta_portion elif dx > 0 and dy < 0: #target_theta = theta_portion target_theta = 360 - theta_portion elif dx < 0 and dy > 0: #target_theta = 180 + theta_portion target_theta = 180 - theta_portion elif dx < 0 and dy < 0: #target_theta = 180 - theta_portion target_theta = 180 + theta_portion print "Target Angle:", target_theta rotation_theta = target_theta - currentAngle if rotation_theta > 180: rotation_theta = rotation_theta - 360 elif rotation_theta < -180: rotation_theta = rotation_theta + 360 print "Rotation Angle:", rotation_theta rotate(rotation_theta) forward_amount = min(distance, 20) fwd_amt(forward_amount) stop() time.sleep(0.05) particles.update_forward(forward_amount) for i in range(0,9): sonar_reading = get_sonar_distance() if sonar_reading < 255: particles.update_probability(sonar_reading) break else: time.sleep(0.1) else: particles.update_probability(get_sonar_distance()) particles.draw()
def turn(deg, orientation): global WHEELRADIUS, speed_left, speed_right, turn_counter if deg < 1: # Angle is sufficiently small to just ignore. :) return turn_counter = 0 #Adjust initial speeds if (orientation == "l"): speed_left = -100 speed_right = 100 elif (orientation == "r"): speed_left = 100 speed_right = -100 else: raise Exception("undefined orientation") #Establish number of spins axle = 7.9 distance = axle * 2 * math.pi * deg / 360 circumference = 2 * math.pi * WHEELRADIUS no_rotations = distance / circumference degrees = no_rotations * 720 BrickPiUpdateValues() offset_1 = BrickPi.Encoder[motor1] offset_2 = BrickPi.Encoder[motor2] #Start turning BrickPi.MotorSpeed[motor1] = speed_left BrickPi.MotorSpeed[motor2] = speed_right #print "deg", degrees, no_rotations BrickPiUpdateValues() #time.sleep(.001) #turned = (BrickPi.Encoder[motor1] - offset_1) * WHEELRADIUS / (2 * axle) if orientation == 'l': particles.update_rotate(deg) else: particles.update_rotate(-deg) particles.update_probability(get_sonar_distance()) particles.draw() while(abs(BrickPi.Encoder[motor1] - offset_1) < degrees and abs(BrickPi.Encoder[motor2] - offset_2) < degrees): BrickPiUpdateValues() time.sleep(.001)
def fwd_amt(distance): global WHEELRADIUS, speed_left, speed_right, particle_counter, circumference BrickPiUpdateValues() offset_1 = BrickPi.Encoder[motor1] offset_2 = BrickPi.Encoder[motor2] speed_left = 200 speed_right = 204 print "- Going forward ", distance, " cm" no_rotations = distance / circumference degrees = no_rotations * 720 BrickPi.MotorSpeed[motor1] = speed_left BrickPi.MotorSpeed[motor2] = speed_right throttle_index = 0 previous_offset = offset_1 while(BrickPi.Encoder[motor1] - offset_1 < degrees and BrickPi.Encoder[motor2] - offset_2 < degrees): # running while loop for no_seconds seconds BrickPiUpdateValues() # Ask BrickPi to update values for sensors/motors adjustValues(degrees, offset_1, offset_2) throttle_index += 1 delta_distance = (circumference * (BrickPi.Encoder[motor1] - previous_offset) / 720) if delta_distance > 20: stop() time.sleep(0.02) particles.update_forward(delta_distance) particles.update_probability(get_sonar_distance()) #print "Moved", delta_distance, "cm - updating particle map" previous_offset = BrickPi.Encoder[motor1] particles.draw() throttle_index = 0 time.sleep(.001) # sleep for 100 ms #needs to update last bit of movement, else particles will be off by up to 10 units BrickPiUpdateValues() delta_distance = (circumference * (BrickPi.Encoder[motor1] - previous_offset) / 720) particles.update_forward(delta_distance) particles.update_probability(get_sonar_distance()) particles.draw()
def turn(deg, orientation): global WHEELRADIUS, speed_left, speed_right, turn_counter turn_counter = 0 #Adjust initial speeds if (orientation == "l"): speed_left = -100 speed_right = 100 elif (orientation == "r"): speed_left = 100 speed_right = -100 else: raise Exception("undefined orientation") #Establish number of spins axle = 7.9 distance = axle * 2 * math.pi * deg / 360 circumference = 2 * math.pi * WHEELRADIUS no_rotations = distance / circumference degrees = no_rotations * 720 BrickPiUpdateValues() offset_1 = BrickPi.Encoder[motor1] offset_2 = BrickPi.Encoder[motor2] #Start turning BrickPi.MotorSpeed[motor1] = speed_left BrickPi.MotorSpeed[motor2] = speed_right #print "deg", degrees, no_rotations BrickPiUpdateValues() #time.sleep(.001) #turned = (BrickPi.Encoder[motor1] - offset_1) * WHEELRADIUS / (2 * axle) particles.update_rotate(deg) particles.draw() while(abs(BrickPi.Encoder[motor1] - offset_1) < degrees and abs(BrickPi.Encoder[motor2] - offset_2) < degrees): BrickPiUpdateValues() time.sleep(.001)
def draw(self): self.bg.convert() if not self.pause: self.bgTime += 1 if self.bgTime + 1 == self.totalGameTime: print(self.player.rect.right // 1024) self.exit = False self.bgTime += 1 # avoid an endless loop self.setGameOver("Time's Up! Press ENTER to exit.") if self.bgTime % 5 == 0: if self.darkening or self.lightening or self.raining: self.bgColor = self.rainSky() else: self.bgColor = self.getTimeColor() color = (self.bgColor[0], self.bgColor[1], self.bgColor[2]) try: self.bg.fill(color) except: self.bg.fill((59, 82, 119)) # blit the background for y in range(32): for x in range(32): self.screen.blit(self.bg, (x * 32, y * 32)) # draw the sun based on "time of day" sun = pygame.transform.scale(load.load_image("sun"), (150, 150)) if 50 + (SCREEN_HEIGHT) * (self.bgTime / self.totalGameTime) <= 400: self.screen.blit(sun, (SCREEN_WIDTH - 150 - 50 * (self.bgTime / self.totalGameTime), 50 + (SCREEN_HEIGHT) * (self.bgTime / self.totalGameTime))) if abs((50 + (SCREEN_HEIGHT) * (self.bgTime / self.totalGameTime)) - 250) <= 2: self.sundownTime = self.bgTime # draw game entities for tree in self.trees: self.screen.blit(tree.image, self.camera.apply(tree)) for e in self.entities: self.screen.blit(e.image, self.camera.apply(e)) for ice in self.ice: self.screen.blit(ice.image, self.camera.apply(ice)) for rock in self.rocks: self.screen.blit(rock.image, self.camera.apply(rock)) for bird in self.birds: self.screen.blit(bird.image, self.camera.apply(bird)) for coin in self.coins: self.screen.blit(coin.image, self.camera.apply(coin)) self.scoreSprite.draw(self.screen) clouds.draw(self.screen) player.drawMessage(self.screen) if self.snowing: self.snow.draw(self.screen) self.checkForRain() particles.draw(self.screen, self.camera) # Draw game info font = load.load_font("Nexa Light", 30) fps = font.render("FPS: " + str(int(self.timer.get_fps())), True, Game.textColor) speed = font.render("Speed: " + str(int(self.groundSpeed)), True, Game.textColor) if self.showFPS: self.screen.blit(fps, (50, 60)) self.screen.blit(speed, (SCREEN_WIDTH - 200, 30)) timeLeft = "Time Left: " timer = font.render(timeLeft, True, Game.textColor) rect = timer.get_rect() rect.right = int(self.screen.get_rect().centerx) rect.top = 30 self.screen.blit(timer, rect) time = font.render(str(self.totalGameTime - self.bgTime), True, Game.textColor) rect = time.get_rect() rect.left = int(self.screen.get_rect().centerx) rect.top = 30 self.screen.blit(time, rect) if self.gameover: if self.exit == False: # when time runs out self.pause = True self.screen.blit(self.gameover_image, self.gameover_rect) else: self.done = True pygame.display.update()