woah1

#R0B0T C0D3!
#
from adafruit_crickit import crickit
import time
import serial
import asyncio
crickit.init_neopixel(8)



## --- Serial Stuff -- ##
ser = serial.Serial('/dev/ttyUSB0')
inputdata = [0,0] # data which is currently being parsed. we have this because it is not yet a full set of readings which will cause the program to cake its trousers.
outputdata = [0,0] # last set of completed readings
counter = 0
buffer = "" # temporary input buffer

## -- Motor stuff -- ##
motorleftspeed = 0					# Variable to keep track of how fast each motor is going
motorrightspeed = 0

leftmotor = crickit.dc_motor_1 		#Just tidying up the names of the motors here
rightmotor = crickit.dc_motor_2

def runmotors(lspeed,rspeed): #function which we use to control the speed of the left and right motors
	if (lspeed != motorleftspeed):
		leftmotor.throttle = lspeed
		motorlspeed = lspeed
	if (rspeed != motorrightspeed):
		rightmotor.throttle = rspeed
		motorrightspeed = rspeed

	if lspeed and rspeed == 0: sopped = True
	else: stopped = False

#stuff that happens when we quit
def quitclean():
        crickit.neopixel.fill((0, 0 , 0))
        ser.close()
        exit()

## -- Turn around bits --

stopped = False # how we know if the robot is moving or not
laststoptime = time.time() # Reference timestamp for turnaround function

def turnaround():
	turnaroundtime = time.time() + 10000 # what time we should stop turning around (ms)
	while time.time() <= turnaroundtime:
		runmotors(-.7,.7) # slide to the left, now take it back now y'all ....



## -- Main While Loop -- ##
try:
    while True:  # at some point you need to make this asyncronous so it can do this in the background while it's thinking aboyut other stuff.

        tmpbuf = ser.read()
        buffer += tmpbuf.decode('utf-8')


        if ":" in buffer:   #if we find a : in our buffer we know that it is the end of that reading

            # print(buffer)
            currentdata = buffer #set the current data we're working on to what we have in the buffer
            currentdata = currentdata.replace(":", "") #remove the : in the buffer so we can convert it into an integer
            currentdata = int(currentdata) #make it in to an integer
            inputdata[counter] = currentdata #add what we have in the buffer into our "inputdata" array
            counter += 1

            buffer = "" # clear the buffer


        elif ";" in buffer: # if we find a ; in the buffer, we know it is the end of the road!

            currentdata = buffer #set the current data we're working on to what we have in the buffer
            currentdata = currentdata.replace(";", "") #remove the : in the buffer so we can convert it into an integer
            currentdata = int(currentdata) #make it in to an integer
            inputdata[counter] = currentdata #add what we have in the buffer into our "inputdata" array

            outputdata = inputdata # we can now set the "outputdata" array to be the same as the "inputdata" as we know it is a complete set
            buffer = "" # clear the buffer so we are ready to start again!
            counter = 0

# Begin Motor Function Control
# Distance is measured in MM

        if outputdata[0] > 200:
			runmotor(.5,.5) # set both motors to .5 speed
			crickit.neopixel.fill((0, 255, 0))
            print("CHOOCH FAST")
        else:
            if outputdata[0] < 100:
				runmotor(0,0)
				crickit.neopixel.fill((255, 0, 0))
				print("NO CHOOCH")

				## Turnaround logic starts here
				now = time.time()
				waittime = 5000 # How long we wait before giving up and turning around in ms
				if (!sopped): laststoptime = now # if this is the first time we've recieved command yet we can let the program now we are stopped now

				if (now - laststoptime) > waittime:
					turnaround() ## if we have waited longer than the amount of patients the robot has we should start turning around

            else:
				runmotor(.3,.3)
				crickit.neopixel.fill((50, 50, 50))
                print("CHOOCH SLOW")

except KeyboardInterrupt:
    quitclean()

# crickit.dc_motor_1.throttle = 0.5
# crickit.dc_motor_2.throttle = 0.5
# time.sleep(3)