#!/usr/bin/env python3
from eye import *

S = 50  # spee [0..100]d
T = 50  #distance threshold [mm] 

# Global speed variables
Fleft = 0
Fright = 0
Bleft = 0
Bright = 0

def Mdrive():
    MOTORDrive(1, Fleft)
    MOTORDrive(2, Fright)
    MOTORDrive(3, Bleft)
    MOTORDrive(4, Bright)

def setspeed(a, b, c, d):
    global Fleft, Fright, Bleft, Bright
    Fleft = a 
    Fright = b
    Bleft = c 
    Bright = d

def addspeed(a, b, c, d):
    global Fleft, Fright, Bleft, Bright
    Fleft += a 
    print("a",Fleft)
    Fright += b
    Bleft += c 
    Bright += d

def main():
    while True:
        left  = PSDGet(1)
        front = PSDGet(2)
        right = PSDGet(3)
        back  = PSDGet(4)
        LCDPrintf("L%d F%d  R%d  B%d\n", left, front, right, back)

        Xdiff = front - back
        Ydiff = left  - right
        setspeed(0, 0, 0, 0)
        if Xdiff >  T: addspeed( S,  S,  S,  S)
        if Xdiff < -T: addspeed(-S, -S, -S, -S)
        if Ydiff >  T: addspeed(-S,  S,  S, -S)
        if Xdiff < -T: addspeed( S, -S, -S,  S)
        Mdrive()


if __name__ == "__main__":
    main()