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()
