#Program to draw a square
# J Fisher 16/10/2013
# Robot UKMARS 3
from UKMARS import SW1, Button, lfront , rfront, Motor,led
import time, utime

leftmotor = Motor("L")
rightmotor = Motor("R")
lspeed = 50
rspeed = 50

# Tuning variables
length = 0.5
spin1 = 0.30
spin2 = 0.90



# subroutine to do 1 side, 1 corner
def move(leftspeed,rightspeed,delay):
    leftmotor.speed(leftspeed)
    rightmotor.speed(rightspeed)
    utime.sleep(delay)
# stop motors
move(0,0,0.05)
# await button
while Button.value()==0:
   time.sleep(0.01)
led.on()
utime.sleep(1)
led.off()
#
#sides 1,2,3
for z in range(6):
    move(lspeed,rspeed,length)           # fwds 10mm
    move(lspeed,-rspeed,spin1)           # spin left 30 degrees
    move(-lspeed,-rspeed,length)         # reverse 10mm
    move(-lspeed,rspeed,spin2)           # spin right 90 degrees

move(0,0,0.05)
# end of program