#!/usr/bin/env python
# coding: latin-1

# Import the libraries we need
import UltraBorg
import PicoBorgRev
import time
import math
import sys

# Settings
distanceMin = 100.0             # Minimum distance in mm, corresponds to DiddyBorg reversing at 100%
distanceMax = 300.0             # Maximum distance in mm, corresponds to DiddyBorg driving at 100%

# Start the UltraBorg
UB = UltraBorg.UltraBorg()      # Create a new UltraBorg object
UB.Init()                       # Set the board up (checks the board is connected)

# Setup the PicoBorg Reverse
PBR = PicoBorgRev.PicoBorgRev()
#PBR.i2cAddress = 0x44                  # Uncomment and change the value if you have changed the board address
PBR.Init()
if not PBR.foundChip:
    boards = PicoBorgRev.ScanForPicoBorgReverse()
    if len(boards) == 0:
        print 'No PicoBorg Reverse found, check you are attached :)'
    else:
        print 'No PicoBorg Reverse at address %02X, but we did find boards:' % (PBR.i2cAddress)
        for board in boards:
            print '    %02X (%d)' % (board, board)
        print 'If you need to change the I²C address change the setup line so it is correct, e.g.'
        print 'PBR.i2cAddress = 0x%02X' % (boards[0])
    sys.exit()
#PBR.SetEpoIgnore(True)                 # Uncomment to disable EPO latch, needed if you do not have a switch / jumper
PBR.SetCommsFailsafe(False)             # Disable the communications failsafe
PBR.ResetEpo()

# Power settings
voltageIn = 12.0                        # Total battery voltage to the PicoBorg Reverse
voltageOut = 6.0                        # Maximum motor voltage

# Setup the power limits
if voltageOut > voltageIn:
    maxPower = 1.0
else:
    maxPower = voltageOut / float(voltageIn)

# Calculate our divisor
distanceDiv = (distanceMax - distanceMin) / 2.0

# Loop over the sequence until the user presses CTRL+C
print 'Press CTRL+C to finish'
try:
    # Initial settings
    speed = 0.0
    driveLeft = 0.0
    driveRight = 0.0
    while True:
        # Read all four ultrasonic values, we use the filtered values so we get accurate readings
        usm1 = UB.GetDistance1()
        usm2 = UB.GetDistance2()
        usm3 = UB.GetDistance3()
        usm4 = UB.GetDistance4()
        # Convert to the nearest millimeter
        usm1 = int(usm1)
        usm2 = int(usm2)
        usm3 = int(usm3)
        usm4 = int(usm4)
        # Determine the speed of DiddyBorg based on the distance readings
        if usm1 != 0:
            speed = ((usm1 - distanceMin) / distanceDiv) - 1.0
            if speed > 1.0:
                speed = 1.0
            elif speed < -1.0:
                speed = -1.0
        driveLeft = speed
        driveRight = speed
        # Set our new speed
        PBR.SetMotor1(driveRight * maxPower)
        PBR.SetMotor2(-driveLeft * maxPower)
        # Wait between readings
        time.sleep(.1)
except KeyboardInterrupt:
    # User has pressed CTRL+C
    PBR.MotorsOff()
    print 'Done'