if n < 0:

return -1

else:

"""Calibrate the robot by performing the action until the left line sensor has changed

to dark three times.

Returns time taken for action"""

count = 0

timestart = 0.0

timeend = 0.0

online = False

# start the motor action, e.g. spinleft(100)

action

# act until see dark line

while pi2go.irLeftLine():

time.sleep(0.01)

online = True

timestart = time.time()

while count < 2:

if not pi2go.irLeftLine():

if not online:

online = True

count += 1

elif online:

online = False

time.sleep(0.01)

timeend = time.time()

pi2go.stop()

logging.debug('Time={}-{}={}'.format(timeend, timestart, (timeend-timestart)/2))

global timespincircle

global timeturncircle

global time10cm

logging.info('Calibrating all')

#timespincircle = calibrate(pi2go.spinLeft(100))

logging.debug('Time spin'.format(timespincircle))

#timeturncircle = calibrate(pi2go.turnForward(50,100))

logging.debug('Time turn'.format(timeturncircle))

time10cm = calibrate(pi2go.forward(100))

"""Get the distance three times and average

not time sensitive"""

d1 = pi2go.getDistance()

time.sleep(0.05)

d2 = pi2go.getDistance()

time.sleep(0.05)

d3 = pi2go.getDistance()

time.sleep(0.05)

# discard outlyers

if abs(d1 - (d2 + d3) / 2) < abs(d1) / 10:

return (d2 + d3) / 2

elif abs(d2 - (d1 + d3) / 2) < abs(d2) / 10:

return (d1 + d3) / 2

elif abs(d3 - (d1 + d2) / 2) < abs(d3) / 10:

return (d1 + d2) / 2

else:

speed = 100

"""Square up to the wall."""

# square up to the wall

time5degrees = timespincircle * 5.0 / 360 * 2

logging.debug('Time 5 degrees={}'.format(time5degrees))

# sense distance of wall

distance_mid = getmyDistance()

logging.debug('Distance_mid={}'.format(distance_mid))

# turn 5 degrees left and sense distance of wall

pi2go.spinLeft(speed)

time.sleep(time5degrees)

pi2go.stop()

distance_left = getmyDistance()

logging.debug('Distance_left={}'.format(distance_left))

# turn back and 5 degrees more right and sense distance of wall

pi2go.spinRight(speed)

time.sleep(2*time5degrees)

pi2go.stop()

distance_right = getmyDistance()

logging.debug('Distance_right={}'.format(distance_right))

# turn back to where we started

pi2go.spinLeft(speed)

time.sleep(time5degrees)

pi2go.stop()

# calculate how much off we are

if distance_left > distance_right: # scan right for minimum

distance_old = distance_left

distance_new = distance_mid

while distance_new < distance_old:

logging.debug('Distance old={} new={}'.format(distance_old, distance_new))

pi2go.spinRight(speed)

time.sleep(time5degrees / 10.0)

pi2go.stop()

distance_old = distance_new

distance_new = getmyDistance()

logging.debug('Distance old={} new={}'.format(distance_old, distance_new))

# spin back

pi2go.spinLeft(speed)

time.sleep(time5degrees / 10.0)

pi2go.stop()

else:

# scan left for minimum

distance_old = distance_right

distance_new = distance_mid

while distance_new < distance_old:

logging.debug('Distance old={} new={}'.format(distance_old, distance_new))

pi2go.spinLeft(speed)

time.sleep(time5degrees / 10.0)

pi2go.stop()

distance_old = distance_new

distance_new = getmyDistance()

logging.debug('Distance old={} new={}'.format(distance_old, distance_new))

# spin back

pi2go.spinRight(speed)

time.sleep(time5degrees / 10.0)

pi2go.stop()

exit()

# turn to face the wall

#if angle <> 0:

# if angle < 0:

# spinleft(50) # angle

# else:

# spinright(50) # angle

# time(t)

# stop()

# return distance to go

distance_mid = getmyDistance()

logging.debug('Distance_mid={}'.format(distance_mid))

"""Follow a black line on a white background"""

#SIM_PULSE = 0.035

TURN_RATE = 50

STEP_RATE = 1 #0.5

SPEED_LINE = 35

logging.debug('Turn-rate={} Step-rate={}'.format(TURN_RATE, STEP_RATE))

# start speed

speed = SPEED_LINE

logging.debug('speed={}'.format(speed))

# start with no turn-rate

turn = 0.0

# count steps on this turn-rate; use to accelerate

step = 1

logging.debug('turn={} step={}'.format(turn, step))

while not pi2go.getSwitch(): #action

#logging.debug('time0={}'.format(time.time()))

# turn left if left sensor detects dark line

if not pi2go.irLeftLine():

#logging.debug('Left')

if turn > 0:

step = 0

#turn = -TURN_RATE

#turn = max(turn - TURN_RATE, -100)

speed = TURN_RATE-20

turn = -TURN_RATE

elif not pi2go.irRightLine():

#logging.debug(' Right')

if turn < 0:

step = 0

#turn = TURN_RATE

#turn = min(turn + TURN_RATE, 100)

speed = TURN_RATE-20

turn = TURN_RATE

else:

# no change for now as line is between sensors

#logging.debug(' None ')

speed = SPEED_LINE

turn = 0

#pass

# accelerate

step += 1

#step = min(step + STEP_RATE, 100)

#logging.debug("turn={} step={}".format(turn, step))

leftspeed = max(min(speed + turn + step * STEP_RATE * my_sign(turn), 100), -100)

rightspeed = max(min(speed - turn + step * STEP_RATE * my_sign(-turn), 100), -100)

#logging.debug("turn={} step={} lspeed={} rspeed={}".format(turn, step, leftspeed, rightspeed))

#logging.debug("lspeed={} rspeed={}".format(leftspeed, rightspeed))

#pi2go.stop()

#time.sleep(0.5)

pi2go.go(leftspeed, rightspeed)

# Ignore the time delay to sample as fast as possible.

#logging.debug('time1={}'.format(time.time()))

#time.sleep(SIM_PULSE)

# IGNORE button code for now.

#if getButton[0] = 1:

# # button is pressed to wait for release to flip action state

# while getButton[0] =1:

# time (0.1)

# action = start-stop(action)

"""Perform whole PiWars Proximity Test challenge."""

MIN_DIST = 1.45

# drive forward 1.3m so that sensors are in range.

# use wheel turn calibration on distance

#pi2go.goBoth(100)

#time.sleep(time10cm*1) # test

#time.sleep(time10cm*13)

pi2go.stop()

# square off to the wall

distance = square_up()

logging.debug('Distance={}'.format(distance))

# proceed towards the wall slowing down as we approach

# speeed 1 (max) at 20cm and 0 (min) at 0.5cm

while distance >= MIN_DIST:

logging.debug('Speed={}'.format((distance - MIN_DIST) / (20 - MIN_DIST) * 100))

pi2go.goBoth(min((distance - MIN_DIST) / (20 - MIN_DIST) * 100, 100))

distance = pi2go.getDistance()

logging.debug('Distance={}'.format(distance))

"""Proceed forward until cross the line."""

pi2go.goBoth(speed)

while not pi2go.irLeftLine():

time.sleep(0.1)

if over:

# get off the line

while pi2go.irLeftLine():

"""Perform three point turn."""

# start speed

speed = 100

# start with no turn-rate

turn = 100.0

# start inside a marked, A3-sized box.

# proceed forward and cross the red line.

gotoline(speed, True)

timestartout = time.time()

# proceed forward and cross the next.

gotoline(speed, True)

# proceed forward and cross the next.

gotoline(speed, True)

timestopout = time.time()

# turn left by 90 degrees (either on the spot or in-motion).

pi2go.spinLeft(turn)

time.sleep(timespincircle*90.0/360)

# drive forward and touch or cross the first black line.

# forward to left line

timestart = time.time()

gotoline(speed, False)

# leave on line

timestop = time.time()

# drive backwards in a straight line and touch or cross the first black line.

# reverse off line and then to right line

gotoline(-speed, True)

gotoline(-speed, True)

# drive forwards to the middle of the turning area.

# move half across distance we have just reversed

pi2go.goBoth(speed)

time.sleep(timestop-timestart)

# turn left by 90 degrees (either on the spot or in-motion).

pi2go.spinLeft(turn)

time.sleep(timespincircle*90.0/360)

# return to the starting box.

# move outbound distance back to start

pi2go.goBoth(speed)

time.sleep(timestopout-timestartout)

"""Straight line speed test."""

# Full speed

speed = 100

# start inside a marked, A3-sized box.

# cross the start line

gotoline(speed, True)

# continue to finish line

gotoline(speed,True)

# go 20cm further (cross the line)

pi2go.goBoth(speed)

time.sleep(time10cm*2)

"""Manual keyboard control code

Based on example at http://learn.pimoroni.com/tutorial/robots/controlling-your-robot-wireless-keyboard

Use Rii mini-keyboard."""

global dev, endpointk, endpointt

# light.red.on()

#Bus 001 Device 001: ID 1d6b:0002 Linux Foundation 2.0 root hub

#dev = usb.core.find(find_all=True, idVendor=0x1d6b, idProduct=0x0002)

dev = usb.core.find(idVendor=USB_VENDOR, idProduct=USB_PRODUCT)

if dev is None:

raise ValueError('Device not found')

#if dev.bDeviceClass != 3:

# raise ValueError('Device is not for Human Consumption')

#find Human Interface Devices, class 3, e.g. keyboard and mouse

for cfg in dev:

#intf = usb.util.find_descriptor(cfg)#, bInterfaceClass=3)

for intf in cfg:

#logging.debug('intf={}'.format(intf))

if dev.is_kernel_driver_active(intf.bInterfaceNumber) is True:

logging.debug('Free from Kernel')

dev.detach_kernel_driver(intf.bInterfaceNumber)

logging.debug('Claiming interface {}'.format(intf.bInterfaceNumber))

usb.util.claim_interface(dev, intf.bInterfaceNumber)

for ep in intf:

if endpointk is None:

endpointk = ep

else:

endpointt = ep

# ep = usb.util.find_descriptor(intf)

# logging.debug('ep={}'.format(ep))

#logging.info('Device has {} interfaces'.format(len(dev)))

#logging.info('dev 0:'.format(dev[0]))

#if len(dev) > 1: logging.info('dev 0:'.format(dev[1]))

#endpointk = dev[0][0][(0,0)][0]

#endpointt = dev[0][1][(1,0)][0]

#if dev.is_kernel_driver_active(USB_IF_KYBD) is True:

# dev.detach_kernel_driver(USB_IF_KYBD)

#logging.debug('claiming keyboard')

#usb.util.claim_interface(dev, USB_IF_KYBD)

#logging.debug('claiming touchpad')

#usb.util.claim_interface(dev, USB_IF_TCHPD)

if not dev is None:

for cfg in dev:

for intf in cfg:

usb.util.release_interface(dev, intf.bInterfaceNumber)

dev.attach_kernel_driver(intf.bInterfaceNumber)

"""Manual control.

Use for straight_line if autonomous mode not working.

Use for obstacle course if autonomous mode not working.

Use for joust and skittles."""

BTN_LEFT = 80

BTN_RIGHT = 79

BTN_DOWN = 81

BTN_UP = 82

BTN_FAST = 28 # Y

BTN_SLOW = 17 # N

BTN_STOP = 44 # Space

BTN_EXIT = 41 # ESC

#explorerhat.light.green.on()

global leftspeed, rightspeed

while not pi2go.getSwitch():

control = None

ctrl_touch = None

try:

control = dev.read(endpointk.bEndpointAddress, endpointk.wMaxPacketSize, USB_TIMEOUT)

ctrl_touch = dev.read(endpointt.bEndpointAddress, endpointt.wMaxPacketSize, USB_TIMEOUT)

#print(control)

except:

pass

if control != None:

logging.debug('control={}'.format(control))

if BTN_DOWN in control:

pi2go.reverse(max(leftspeed, rightspeed))

if BTN_UP in control:

pi2go.forward(max(leftspeed, rightspeed))

if BTN_LEFT in control:

pi2go.spinLeft(max(leftspeed, rightspeed))

if BTN_RIGHT in control:

pi2go.spinRight(max(leftspeed, rightspeed))

if BTN_FAST in control:

leftspeed = min(leftspeed + 10, 100)

rightspeed = min(rightspeed + 10, 100)

logging.debug('leftspeed={} rightspeed={}'.format(leftspeed, rightspeed))

if BTN_SLOW in control:

leftspeed = max(leftspeed - 10, 0)

rightspeed = max(rightspeed -10, 0)

logging.debug('leftspeed={} rightspeed={}'.format(leftspeed, rightspeed))

if BTN_STOP in control:

pi2go.stop()

if BTN_EXIT in control:

break

if ctrl_touch != None:

logging.debug('ctrl touch={}'.format(ctrl_touch))

time.sleep(0.02)

"""Perform a PiWars challenge, autonomous or manual based on keypress

Current options are:

follow_line

proximity_test

three_point_turn

manual"""

global dev, endpointk

BTN_C = 6

BTN_F = 9

BTN_P = 19

BTN_S = 22

BTN_T = 23

BTN_M = 16

BTN_Q = 20

logging.info('Start selection')

logging.debug('C F P S T M Q')

while True:

control = None

try:

control = dev.read(endpointk.bEndpointAddress, endpointk.wMaxPacketSize, USB_TIMEOUT)

logging.debug('Control={}'.format(control))

except:

pass

if control != None:

if BTN_C in control:

logging.debug('C for Calibrate')

calibrate_all()

if BTN_F in control:

logging.debug('F for Following')

follow_line()

if BTN_P in control:

logging.debug('P for Proximity Test')

proximity_test()

if BTN_S in control:

logging.debug('S for Straight Line')

straight_line()

if BTN_T in control:

logging.debug('T for Three-point Turn')

three_point_turn()

if BTN_M in control:

logging.debug('M for Manual')

manual()

if BTN_Q in control:

logging.debug('Q for Quit')

break

time.sleep(0.02)