"""
Test the reading of sensor data
"""
import Communications as com
import time
for i in xrange(2000):
data = com.request_all()
print data
#print data['SONAR_C']
time.sleep(0.1)
com.set_drive_motors(20,20)
com.reset_robot(78,12,0)
for i in xrange(500):
# pos = -100 + int(random() * 200)
pos += d
if pos >= 120 or pos <= -120:
d *= -1
com.set_motor(3, pos)
ang = pos / 2
time.sleep(0.01)
sensors = com.request_all()
dist = sensors["LIDAR"]
print dist
time.sleep(0.01)
x = int(origin_x + sin(radians(ang)) * dist)
y = int(origin_y - cos(radians(ang)) * dist)
screen.set_at((x, y), (255, 255, 255))
pygame.display.update()
