Code Listing – Interactive GPIO Robot

				
					# Robot Controller Program
# Training Mode:
# accepts motor control commands, calculates the time delta_t between commands,
# and saves to the track file a line containing the command the value of delta_t
# associated with it.

import time
import os
import sys
import glob
from pynput import keyboard
from datetime import timedelta, datetime
from gpiozero import Robot
global robby
global trk_file
global last_key_event_was_release
global t_start
global t_now
global delta_t
global track_record

robby = Robot(left=(9,10), right=(7,8))
script_path = os.path.realpath(__file__)
#print ("SCRIPT FILE PATH:\t" + script_path + "\n")
wrk_dir = os.path.dirname(script_path)
#print("WORKING DIRECTORY:\t" + wrk_dir + "\n")
trk_dir = os.path.join(wrk_dir, "tracks")
trk_file = None
t_start = time.time()
t_now = t_start
delta_t = t_now - t_start
last_key_event_was_release = False

def record_command(cmd)
    if (last_key_event_was_release == True):
        last_key_event_was_release = False
        track_record.write(cmd+("\t") + str(time.time()) +"\n")
        track_record.flush()

def on_press(key):
    global track_record
    global last_key_event_was_release
    try:
        if key == keyboard.Key.up:
            robby.forward()
            record_command("f")
        elif key == keyboard.Key.down:
            robby.backward()
            record_command("b")
        elif key == keyboard.Key.left:
            robby.left()
            record_command("l")
        elif key == keyboard.Key.right:
            robby.right()
            record_command("r")
    except AttributeError as error:
        print(error)

def on_release(key):
    global track_record
    global last_key_event_was_release
    #print('{0} released; car stopped'.format(key))
    last_key_event_was_release = True
    robby.stop()
    print("Release time (robot.stopped) = ", t_now)
    track_record.write("s"+("\t") + str(time.time()) +"\n")
    track_record.flush()


def do_command(command):
    if command == 's':
        robby.stop()
    elif command == 'f':
        robby.forward()
    elif command == 'b':
        robby.backward()
    elif command == 'l':
        robby.left()
    elif command == 'r':
        robby.right()
    elif command == 'q':
        robby.stop()
    else:
        print("UNKNOWN COMMAND" )

def training_mode(trk_dir):
    print("Start Training - Use Arrow Keys for Car Controls: - Press and Hold:")
    print ("\tUp=forward; Down=backward; Left=Left; Right=Right")
    print("Release Arrow Key to Stop")
    print("ENTER letter 'q' to stop training and quit")
    global t_start
    global track_record
    os.chdir(trk_dir)
    track_record = open("RC-"+str(t_start)+".trk", "w") #time-tagged file names
    #track_record = open("RC-"+".trk", "w") #untagged / common file name
    robby.stop()	#Initial State in Training Mode
    listener = keyboard.Listener(
        on_press=on_press,
        on_release=on_release,
        supress=False)
    listener.start()
    t_previous = t_start
    last_key_event = None
    t_start = time.time()
    t_now = t_start

    while True:
        command = input()
        #track_record.write(str(command) +("\t") + str(delta_t) +"\n")
        #track_record.flush()
        if command == 'q':
            break
    track_record.close()
    listener.stop()
    print("Training Mode Stopped\n")

def get_trk_file(trk_dir):
    old_wd=os.getcwd()
    os.chdir(trk_dir)
    cwd=os.getcwd()
    print(cwd)

    file_paths = []
    for root, dirs, files in os.walk(cwd):
        for idx, file in enumerate(files,1):
            if file.endswith(".trk"):
                filepath=os.path.join(root, file)
                file_paths.append(file)	
    file_paths.sort(reverse=True)
    while True:
        for cnt, fileName in enumerate(file_paths):
            sys.stdout.write("[%d] %s\n\r" % (cnt, fileName))
        try:
            idx_choice = int(input("Select trk file[0-%s]: " % cnt))
            file_choice = file_paths[idx_choice]
            os.chdir(old_wd)
            return file_choice
        except:
            print("List Index Error ... MAKE A DIFFERENT SELECTION")
    return file_paths[0] # if we get here, return first file in file_paths list


def playback_mode(trk_dir):
    print("Start Playback")
    trk_file = get_trk_file(trk_dir)
    os.chdir(trk_dir)
    print(trk_file)
    try:
        with open(str(trk_file),"r") as track_record:
            robby.stop()
            line = track_record.readline()
            fields=line.split()
            print("Command: \t" , fields[0] , "\tdelta_t: \t" , 0)
            do_command(fields[0])
            t_previous = fields[1]
            for line in track_record:
                print(line)
                fields=line.split()
                delta_t = float(fields[1])-float(t_previous)
                print("Command: \t" , fields[0] , "\tdelta_t: \t" , delta_t)
                do_command(fields[0])
                t_previous = fields[1]
                time.sleep(float(delta_t))
    except FileExistsError as error:
        print(error)
        print("WTF?")
    track_record.close()
    robby.stop()
    print("Playback Stopped\n")

#================================
print("Welcome to Robot Car Controller")


try:
    os.mkdir(trk_dir)
except OSError as error:
    if error.errno == 17:
        print("Track Directory already exists; OSError safely ignored")
    else:
        print(error)
print("TRACK DIRECTORY:\t" + trk_dir )
print("\n")

# initialize_robot()
#robby = Robot( left(7,8), right(9,10) )
print ("> Initializing Robot")

while True:
    mode = input("Input Operating Mode, one of {TRAIN, PLAYBACK, QUIT}")
    if mode == "TRAIN":
        training_mode(trk_dir)
        continue
    elif mode == "PLAYBACK":
        playback_mode(trk_dir)
        continue
    elif mode == "QUIT":
        break
    print("Unrecognized Mode Request; TRY AGAIN\n")

print("\nExiting Robot Car Controller\n\n")