import RPi.GPIO as GPIO
import math
import time

GPIO.setwarnings(False)
GPIO.setmode(GPIO.BOARD)

Motorforward = 37
Motorturning = 36
GPIO.setup(Motorforward,GPIO.OUT)
GPIO.setup(Motorturning,GPIO.OUT)
GPIO.output(Motorforward,GPIO.HIGH)
GPIO.output(Motorturning,GPIO.HIGH)
time.sleep(1)		
x = float (input("Enter the X coordinate: "))
y = float (input("Enter the Y coordinate: "))		
r=float(math.sqrt((x*x)+(y*y)))
if((x!=0)and(y==0)):
	theta = 1.7
	thetadegrees = 90
if((x!=0)and(y!=0)):
	theta = float(math.atan(y/x))
	thetadegree = float((theta*180)/3.14)
if(r<0):
	r=(r*(-1))
print "Resultant vector will be"
print r
if((x==0) and (y!=0)):
	thetadegrees = 0	

if((x<0) or (y<0)):
	theta = 0
	thetadegree = 180 - thetadegree
if((x==0) and (y==0)):
	thetadegree = 0
	tturning = 0
print "Angle Of Turning will be from the current position"
tforward = float (6.311*r)
tturning = float (0.0311*thetadegree)

print "Estimated time for travelling forward"
print tforward
print "Estimated time for turning"
print  tturning
GPIO.output(Motorturning,GPIO.LOW)
print "Robot is turning!"
time.sleep(tturning)
GPIO.output(Motorturning,GPIO.LOW)
GPIO.output(Motorforward,GPIO.LOW)
print "Robot is moving!"
time.sleep(tforward)
GPIO.output(Motorforward,GPIO.HIGH)
GPIO.output(Motorturning,GPIO.HIGH)
print "Robot has reached the destination"