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

def Mdrive(txt, drive, steer):
    '''Print txt and drive motors and encoders for 1.5s'''
    LCDPrintf("%s\n", txt)
    MOTORDrive(1, drive)
    #  MOTORDrive(2, drive)
    SERVOSet  (1, steer)
    OSWait(1500)
    #  LCDPrintf("Enc. %5d\n", ENCODERRead(1))
 
def main():
    LCDPrintf("Ackermann Steer\n")
    Mdrive("Forward",     60, 127)
    Mdrive("Backward",   -60, 127)
    Mdrive("Left Curve",  60,   0)
    Mdrive("Right Curve", 60, 255)
    Mdrive("Stop",         0,  0)

if __name__ == "__main__":
    main()