import time
import pygame
from pygame.locals import *
import RPi.GPIO as GPIO
if __name__ == "__main__":
#pwm = PWM(0x40)
#pwm.setPWMFreq(60) # Set frequency to 60 Hz
#pwm = PWM(0x40, debug=True)
servoDriver = ServoDriver()
screen = pygame.display.set_mode((700, 700))
m = Mapping(screen)
motor = MotorDriver()
nav = Navigation(motor,screen)
print "setting up rangers"
r = StepSweep(servoDriver)
running = True
wait_for_input = True
print "reading..."
while running:
screen.fill((255,255,255))
wait_for_input = True
r.servoToStart()
time.sleep(1)
rangeData = []
rangeData.append(RangeData(10,120))
rangeData.append(RangeData(100,90))
nav.draw(offset,0,m.getLastPosition(),m.getLastRotation())
m.drawPosition(offset)
pygame.display.flip()
if __name__ == "__main__":
#pwm = PWM(0x40)
#pwm.setPWMFreq(60) # Set frequency to 60 Hz
#pwm = PWM(0x40, debug=True)
servoDriver = ServoDriver()
screen = pygame.display.set_mode((700, 700))
pygame.display.init()
m = Mapping(screen);
print "setting up rangers"
r = StepSweep(servoDriver)
motor = MotorDriver()
nav = Navigation(motor,screen)
running = True
wait_for_results = False
offset = [0,0]
r.start()
r.servoToStart()
time.sleep(0.7)
print "reading..."
while running:
if wait_for_results:
time.sleep(0.02)
if r.isDone():
