import cv2
import threading
from time import sleep
import time

import RPi.GPIO as GPIO
GPIO.setmode(GPIO.BCM)

GPIO.setup(17, GPIO.OUT)
GPIO.setup(27, GPIO.OUT)

import board
import busio
import digitalio
from adafruit_mcp230xx.mcp23017 import MCP23017
# Initialize the I2C bus:
i2c = busio.I2C(board.SCL, board.SDA)
mcp = MCP23017(i2c, address=0x23)
mcp2 = MCP23017(i2c, address=0x27)

from adafruit_servokit import ServoKit
kit = ServoKit(channels=16)
global start
start=-5
global end
global prevdata
#define pins for relays
pin0 = mcp.get_pin(0)
pin1 = mcp.get_pin(1)
pin2 = mcp.get_pin(2)
pin3 = mcp.get_pin(3)
pin4 = mcp.get_pin(4)
pin5 = mcp.get_pin(5)
pin6 = mcp.get_pin(6)
pin7 = mcp.get_pin(7)
pin8 = mcp.get_pin(8)
pin9 = mcp.get_pin(9)
pin10 = mcp.get_pin(10)
pin11 = mcp.get_pin(11)
pin12 = mcp.get_pin(12)
pin13 = mcp.get_pin(13)
pin14 = mcp.get_pin(14)
pin15 = mcp.get_pin(15)


STEP1 = mcp2.get_pin(0)
DIR1 = mcp2.get_pin(1)
EN1 = mcp2.get_pin(2)

STEP2 = mcp2.get_pin(3)
DIR2 = mcp2.get_pin(4)
EN2 = mcp2.get_pin(5)

STEP3 = mcp2.get_pin(6)
DIR3 = mcp2.get_pin(7)
EN3 = mcp2.get_pin(8)

STEP4 = mcp2.get_pin(9)
DIR4 = mcp2.get_pin(10)
EN4 = mcp2.get_pin(11)

STEP5 = mcp2.get_pin(12)
DIR5 = mcp2.get_pin(13)
EN5 = mcp2.get_pin(14)



pin0.direction = digitalio.Direction.OUTPUT
pin1.direction = digitalio.Direction.OUTPUT
pin2.direction = digitalio.Direction.OUTPUT
pin3.direction = digitalio.Direction.OUTPUT
pin4.direction = digitalio.Direction.OUTPUT
pin5.direction = digitalio.Direction.OUTPUT
pin6.direction = digitalio.Direction.OUTPUT
pin7.direction = digitalio.Direction.OUTPUT
pin8.direction = digitalio.Direction.OUTPUT
pin9.direction = digitalio.Direction.OUTPUT
pin10.direction = digitalio.Direction.OUTPUT
pin11.direction = digitalio.Direction.OUTPUT
pin12.direction = digitalio.Direction.OUTPUT
pin13.direction = digitalio.Direction.OUTPUT
pin14.direction = digitalio.Direction.OUTPUT
pin15.direction = digitalio.Direction.OUTPUT

STEP1.switch_to_output(value=True)
DIR1.switch_to_output(value=True)
EN1.switch_to_output(value=True)
STEP2.switch_to_output(value=True)
DIR2.switch_to_output(value=True)
EN2.switch_to_output(value=True)
STEP3.switch_to_output(value=True)
DIR3.switch_to_output(value=True)
EN3.switch_to_output(value=True)
STEP4.switch_to_output(value=True)
DIR4.switch_to_output(value=True)
EN4.switch_to_output(value=True)
STEP5.switch_to_output(value=True)
DIR5.switch_to_output(value=True)
EN5.switch_to_output(value=True)

pin0.value = True
pin1.value = True
pin2.value = True
pin3.value = True
pin4.value = True
pin5.value = True
pin6.value = True
pin7.value = True
pin8.value = True
pin9.value = True
pin10.value = True
pin11.value = True
pin12.value = True
pin13.value = True
pin14.value = True
pin15.value = True

kit.servo[0].actuation_range = 270
kit.servo[1].actuation_range = 270
kit.servo[2].actuation_range = 270
kit.servo[3].actuation_range = 270
kit.servo[4].actuation_range = 270
kit.servo[5].actuation_range = 270
kit.servo[6].actuation_range = 270
kit.servo[7].actuation_range = 270
kit.servo[8].actuation_range = 270
kit.servo[9].actuation_range = 270
kit.servo[10].actuation_range = 270
kit.servo[11].actuation_range = 270
kit.servo[12].actuation_range = 270
kit.servo[13].actuation_range = 270
kit.servo[14].actuation_range = 270
kit.servo[15].actuation_range = 270

MAX_PWM=2650
MIN_PWM=650
kit.servo[0].set_pulse_width_range(MIN_PWM, MAX_PWM)
kit.servo[1].set_pulse_width_range(MIN_PWM, MAX_PWM)
kit.servo[2].set_pulse_width_range(MIN_PWM, MAX_PWM)
kit.servo[3].set_pulse_width_range(MIN_PWM, MAX_PWM)
kit.servo[4].set_pulse_width_range(MIN_PWM, MAX_PWM)
kit.servo[5].set_pulse_width_range(MIN_PWM, MAX_PWM)
kit.servo[6].set_pulse_width_range(MIN_PWM, MAX_PWM)
kit.servo[7].set_pulse_width_range(MIN_PWM, MAX_PWM)
kit.servo[8].set_pulse_width_range(MIN_PWM, MAX_PWM)
kit.servo[9].set_pulse_width_range(MIN_PWM, MAX_PWM)
kit.servo[10].set_pulse_width_range(MIN_PWM, MAX_PWM)
kit.servo[11].set_pulse_width_range(MIN_PWM, MAX_PWM)
kit.servo[12].set_pulse_width_range(MIN_PWM, MAX_PWM)
kit.servo[13].set_pulse_width_range(MIN_PWM, MAX_PWM)
kit.servo[14].set_pulse_width_range(MIN_PWM, MAX_PWM)
kit.servo[15].set_pulse_width_range(MIN_PWM, MAX_PWM)

cap = cv2.VideoCapture(0)
detector = cv2.QRCodeDetector()

# pins for stepper 1
# coil_A_1_pin = 17
# coil_A_2_pin = 27
# coil_B_1_pin = 22
# coil_B_2_pin = 10
# GPIO.setup(coil_A_1_pin, GPIO.OUT)
# GPIO.setup(coil_A_2_pin, GPIO.OUT)
# GPIO.setup(coil_B_1_pin, GPIO.OUT)
# GPIO.setup(coil_B_2_pin, GPIO.OUT)



class myThread (threading.Thread):
    def __init__(self, threadID, name, code):
        threading.Thread.__init__(self)
        self.threadID = threadID
        self.name = name
        self.code = code
    def run(self):
        print ("Starting " + self.name)
        if len(self.code) ==9:
            self.run_servo()
        elif len(self.code)==15:
            self.run_stepper()
        elif len(self.code)==10:
            self.run_relay()
        print ("Exiting " + self.name)
    def run_servo(self):
        print("Starting Servo: "+ str(self.code))
        print ("%s" % ( time.ctime(time.time())))
        self.servo_initial_delay = self.code[0:2]
        print("servo: Initial Delay", self.servo_initial_delay)
        self.servo_name = self.code[2:4]
        print ("Servo Name: ", self.servo_name)
        self.servo_multipler = self.code[4:6]
        print ("Servo Multiplier: ", self.servo_multipler)
        self.servo_angle = self.code[6:7]
        self.servo_angle = (ord(self.servo_angle)-96)*15
        print ("Servo Angle: ", self.servo_angle)
        self.servo_delay = int(self.code[7:9])
        if self.servo_name == "01":
            sleep(int(self.servo_initial_delay)) # sleep for initial delay seconds
            for i in range(int(self.servo_multipler)):
                kit.servo[0].angle = self.servo_angle # go to specified angle channel 0
                sleep(self.servo_delay)
                sleep(1.5)
                kit.servo[0].angle = 0 # go to 0 angle channel 0
                sleep(1.5)
                print("cycle:", i)
        elif self.servo_name == "02":
            sleep(int(self.servo_initial_delay)) # sleep for initial delay seconds
            for i in range(int(self.servo_multipler)):
                kit.servo[1].angle = self.servo_angle # go to specified angle channel 0
                sleep(self.servo_delay)
                sleep(1.5)
                kit.servo[1].angle = 0 # go to 0 angle channel 0
                sleep(1.5)
        elif self.servo_name == "03":
            sleep(int(self.servo_initial_delay)) # sleep for initial delay seconds
            for i in range(int(self.servo_multipler)):
                kit.servo[2].angle = self.servo_angle # go to specified angle channel 0
                sleep(self.servo_delay)
                sleep(1.5)
                kit.servo[2].angle = 0 # go to 0 angle channel 0
                sleep(1.5)
        elif self.servo_name == "04":
            sleep(int(self.servo_initial_delay)) # sleep for initial delay seconds
            for i in range(int(self.servo_multipler)):
                kit.servo[3].angle = self.servo_angle # go to specified angle channel 0
                sleep(self.servo_delay)
                sleep(1.5)
                kit.servo[3].angle = 0 # go to 0 angle channel 0
                sleep(1.5)
        elif self.servo_name == "05":
            sleep(int(self.servo_initial_delay)) # sleep for initial delay seconds
            for i in range(int(self.servo_multipler)):
                kit.servo[4].angle = self.servo_angle # go to specified angle channel 0
                sleep(self.servo_delay)
                sleep(1.5)
                kit.servo[4].angle = 0 # go to 0 angle channel 0
                sleep(1.5)
        elif self.servo_name == "06":
            sleep(int(self.servo_initial_delay)) # sleep for initial delay seconds
            for i in range(int(self.servo_multipler)):
                kit.servo[5].angle = self.servo_angle # go to specified angle channel 0
                sleep(self.servo_delay)
                sleep(1.5)
                kit.servo[5].angle = 0 # go to 0 angle channel 0
                sleep(1.5)
        elif self.servo_name == "07":
            sleep(int(self.servo_initial_delay)) # sleep for initial delay seconds
            for i in range(int(self.servo_multipler)):
                kit.servo[6].angle = self.servo_angle # go to specified angle channel 0
                sleep(self.servo_delay)
                sleep(1.5)
                kit.servo[6].angle = 0 # go to 0 angle channel 0
                sleep(1.5)
        elif self.servo_name == "08":
            sleep(int(self.servo_initial_delay)) # sleep for initial delay seconds
            for i in range(int(self.servo_multipler)):
                kit.servo[7].angle = self.servo_angle # go to specified angle channel 0
                sleep(self.servo_delay)
                sleep(1.5)
                kit.servo[7].angle = 0 # go to 0 angle channel 0
                sleep(1.5)
        elif self.servo_name == "09":
            sleep(int(self.servo_initial_delay)) # sleep for initial delay seconds
            for i in range(int(self.servo_multipler)):
                kit.servo[8].angle = self.servo_angle # go to specified angle channel 0
                sleep(self.servo_delay)
                sleep(1.5)
                kit.servo[8].angle = 0 # go to 0 angle channel 0
                sleep(1.5)
        elif self.servo_name == "10":
            sleep(int(self.servo_initial_delay)) # sleep for initial delay seconds
            for i in range(int(self.servo_multipler)):
                kit.servo[9].angle = self.servo_angle # go to specified angle channel 0
                sleep(self.servo_delay)
                sleep(1.5)
                kit.servo[9].angle = 0 # go to 0 angle channel 0
                sleep(1.5)
        elif self.servo_name == "11":
            sleep(int(self.servo_initial_delay)) # sleep for initial delay seconds
            for i in range(int(self.servo_multipler)):
                kit.servo[10].angle = self.servo_angle # go to specified angle channel 0
                sleep(self.servo_delay)
                sleep(1.5)
                kit.servo[10].angle = 0 # go to 0 angle channel 0
                sleep(1.5)
        elif self.servo_name == "12":
            sleep(int(self.servo_initial_delay)) # sleep for initial delay seconds
            for i in range(int(self.servo_multipler)):
                kit.servo[11].angle = self.servo_angle # go to specified angle channel 0
                sleep(self.servo_delay)
                sleep(1.5)
                kit.servo[11].angle = 0 # go to 0 angle channel 0
                sleep(1.5)
        elif self.servo_name == "13":
            sleep(int(self.servo_initial_delay)) # sleep for initial delay seconds
            for i in range(int(self.servo_multipler)):
                kit.servo[12].angle = self.servo_angle # go to specified angle channel 0
                sleep(self.servo_delay)
                sleep(1.5)
                kit.servo[12].angle = 0 # go to 0 angle channel
                sleep(1.5)
        elif self.servo_name == "14":
            sleep(int(self.servo_initial_delay)) # sleep for initial delay seconds
            for i in range(int(self.servo_multipler)):
                kit.servo[13].angle = self.servo_angle # go to specified angle channel 0
                sleep(self.servo_delay)
                sleep(1.5)
                kit.servo[13].angle = 0 # go to 0 angle channel 0
                sleep(1.5)
        elif self.servo_name == "15":
            sleep(int(self.servo_initial_delay)) # sleep for initial delay seconds
            for i in range(int(self.servo_multipler)):
                kit.servo[14].angle = self.servo_angle # go to specified angle channel 0
                sleep(self.servo_delay)
                sleep(1.5)
                kit.servo[14].angle = 0 # go to 0 angle channel 0
                sleep(1.5)
        elif self.servo_name == "16":
            sleep(int(self.servo_initial_delay)) # sleep for initial delay seconds
            for i in range(int(self.servo_multipler)):
                kit.servo[15].angle = self.servo_angle # go to specified angle channel 0
                sleep(self.servo_delay)
                sleep(1.5)
                kit.servo[15].angle = 0 # go to 0 angle channel 0
                sleep(1.5)
    def run_relay(self):
        print("Starting Relay: "+ str(self.code))
        print ("%s" % ( time.ctime(time.time())))
        self.relay_initial_delay = self.code[0:2]
        print("Relay: Initial Delay", self.relay_initial_delay)
        self.relay_name = self.code[2:4]
        print("Relay Name: ", self.relay_name)
        self.relay_seconds = self.code[4:6]
        print("Relay Seconnds: ", self.relay_seconds)
        self.relay_minutes = self.code[7:9]
        print("Relay Minutes: ", self.relay_minutes)
        self.relay_seconds = int(self.relay_seconds) + int(self.relay_minutes)*60
        if self.relay_name == "01":
            sleep(int(self.relay_initial_delay)) # sleep for initial delay
            pin0.value = False
            sleep(self.relay_seconds)
            pin0.value = True
        elif self.relay_name == "02":
            sleep(int(self.relay_initial_delay)) # sleep for initial delay
            pin1.value = False
            sleep(self.relay_seconds)
            pin1.value = True
        elif self.relay_name == "03":
            sleep(int(self.relay_initial_delay)) # sleep for initial delay
            pin2.value = False
            sleep(self.relay_seconds)
            pin2.value = True
        elif self.relay_name == "04":
            sleep(int(self.relay_initial_delay)) # sleep for initial delay
            pin3.value = False
            sleep(self.relay_seconds)
            pin3.value = True
        elif self.relay_name == "05":
            sleep(int(self.relay_initial_delay)) # sleep for initial delay
            pin4.value = False
            sleep(self.relay_seconds)
            pin4.value = True
        elif self.relay_name == "06":
            sleep(int(self.relay_initial_delay)) # sleep for initial delay
            pin5.value = False
            sleep(self.relay_seconds)
            pin5.value = True
        elif self.relay_name == "07":
            sleep(int(self.relay_initial_delay)) # sleep for initial delay
            pin6.value = False
            sleep(self.relay_seconds)
            pin6.value = True
        elif self.relay_name == "08":
            sleep(int(self.relay_initial_delay)) # sleep for initial delay
            pin7.value = False
            sleep(self.relay_seconds)
            pin7.value = True
        elif self.relay_name == "09":
            sleep(int(self.relay_initial_delay)) # sleep for initial delay
            pin8.value = False
            sleep(self.relay_seconds)
            pin8.value = True
        elif self.relay_name == "10":
            sleep(int(self.relay_initial_delay)) # sleep for initial delay
            pin9.value = False
            sleep(self.relay_seconds)
            pin9.value = True
        elif self.relay_name == "11":
            sleep(int(self.relay_initial_delay)) # sleep for initial delay
            pin10.value = False
            sleep(self.relay_seconds)
            pin10.value = True
        elif self.relay_name == "12":
            sleep(int(self.relay_initial_delay)) # sleep for initial delay
            pin11.value = False
            sleep(self.relay_seconds)
            pin11.value = True
        elif self.relay_name == "13":
            sleep(int(self.relay_initial_delay)) # sleep for initial delay
            pin12.value = False
            sleep(self.relay_seconds)
            pin12.value = True
        elif self.relay_name == "14":
            sleep(int(self.relay_initial_delay)) # sleep for initial delay
            pin13.value = False
            sleep(self.relay_seconds)
            pin13.value = True
        elif self.relay_name == "15":
            sleep(int(self.relay_initial_delay)) # sleep for initial delay
            pin14.value = False
            sleep(self.relay_seconds)
            pin14.value = True
        elif self.relay_name == "16":
            sleep(int(self.relay_initial_delay)) # sleep for initial delay
            pin15.value = False
            sleep(self.relay_seconds)
            pin15.value = True
    def run_stepper(self):
        print("Starting Stepper: "+ str(self.code))
        print ("%s" % ( time.ctime(time.time())))
        self.stepper_initial_delay = self.code[0:2]
        print("Stepper: Initial Delay", self.stepper_initial_delay)
        self.stepper_name = self.code[2:4]
        print("Stepper Name: ", self.stepper_name)
        self.stepper_rotation_clockwise = self.code[5:7]
        print("Stepper: Clockwise Rotation : ", self.stepper_rotation_clockwise)
        self.stepper_hold_seconds = self.code[8:10]
        print("Stepper: Hold seconds: ", self.stepper_hold_seconds)
        self.stepper_rotation_counter_clockwise = self.code[10:12]
        print("Stepper: Counter Clockwise Rotation: ", self.stepper_rotation_counter_clockwise)
        self.Stepper_repeat = self.code[14]
        print("Stepper: Repeat: ", self.Stepper_repeat)
        if self.stepper_name == "01": # Black(A+), Green(A-), Red(B+), Blue(B-)
            self.delay = 0.001
            self.steps = 200
            sleep(int(self.stepper_initial_delay)) # sleep for initial delay
            EN1.value = False
            for i in range(int(self.Stepper_repeat)+1):
                DIR1.value = True
                for j in range(self.steps*int(self.stepper_rotation_clockwise)): # Run the setps for clockwise
                    # STEP1.value = True
                    GPIO.output(17, 1)
                    time.sleep(self.delay)
                    # STEP1.value = False
                    GPIO.output(17, 0)
                    time.sleep(self.delay)
                sleep(int(self.stepper_hold_seconds)) # Hold before counter clockwise
                DIR1.value = False
                for j in range(self.steps*int(self.stepper_rotation_counter_clockwise)): # Reverse previous step sequence to reverse motor direction
                    # STEP1.value = True
                    GPIO.output(17, 1)
                    time.sleep(self.delay)
                    # STEP1.value = False
                    GPIO.output(17, 0)
                    time.sleep(self.delay)
            EN1.value = True
        elif self.stepper_name == "02":
            self.delay = 0.001
            self.steps = 200
            sleep(int(self.stepper_initial_delay)) # sleep for initial delay
            EN2.value = False
            for i in range(int(self.Stepper_repeat)+1):
                DIR2.value = True
                for j in range(self.steps*int(self.stepper_rotation_clockwise)): # Run the setps for clockwise
                    # STEP2.value = True
                    GPIO.output(27, 1)
                    time.sleep(self.delay)
                    # STEP2.value = False
                    GPIO.output(27, 0)
                    time.sleep(self.delay)
                sleep(int(self.stepper_hold_seconds)) # Hold before counter clockwise
                DIR2.value = False
                for j in range(self.steps*int(self.stepper_rotation_counter_clockwise)): # Reverse previous step sequence to reverse motor direction
                    # STEP2.value = True
                    GPIO.output(27, 1)
                    time.sleep(self.delay)
                    # STEP2.value = False
                    GPIO.output(27, 0)
                    time.sleep(self.delay)
            EN2.value = True
        elif self.stepper_name == "03":
            print("No pins defined")

'''
def run_servo(code):
    print("Starting Servo: "+ str(code))
    print ("%s" % ( time.ctime(time.time())))
    servo_initial_delay = code[0:2]
    print("servo: Initial Delay", servo_initial_delay)
    servo_name = code[2:4]
    print ("Servo Name: ", servo_name)
    servo_multipler = code[4:6]
    print ("Servo Multiplier: ", servo_multipler)
    servo_angle = code[6:7]
    servo_angle = (ord(servo_angle)-96)*15
    print ("Servo Angle: ", servo_angle)
    servo_delay = int(code[7:9])
    if servo_name == "01":
        sleep(int(servo_initial_delay)) # sleep for initial delay seconds
        for i in range(int(servo_multipler)):
            kit.servo[0].angle = servo_angle # go to specified angle channel 0
            sleep(servo_delay)
            kit.servo[0].angle = 0 # go to 0 angle channel 0
    elif servo_name == "02":
        sleep(int(servo_initial_delay)) # sleep for initial delay seconds
        for i in range(int(servo_multipler)):
            kit.servo[1].angle = servo_angle # go to specified angle channel 0
            sleep(servo_delay)
            kit.servo[1].angle = 0 # go to 0 angle channel 0
    elif servo_name == "03":
        sleep(int(servo_initial_delay)) # sleep for initial delay seconds
        for i in range(int(servo_multipler)):
            kit.servo[2].angle = servo_angle # go to specified angle channel 0
            sleep(servo_delay)
            kit.servo[2].angle = 0 # go to 0 angle channel 0
    elif servo_name == "04":
        sleep(int(servo_initial_delay)) # sleep for initial delay seconds
        for i in range(int(servo_multipler)):
            kit.servo[3].angle = servo_angle # go to specified angle channel 0
            sleep(servo_delay)
            kit.servo[3].angle = 0 # go to 0 angle channel 0
    elif servo_name == "05":
        sleep(int(servo_initial_delay)) # sleep for initial delay seconds
        for i in range(int(servo_multipler)):
            kit.servo[4].angle = servo_angle # go to specified angle channel 0
            sleep(servo_delay)
            kit.servo[4].angle = 0 # go to 0 angle channel 0
    elif servo_name == "06":
        sleep(int(servo_initial_delay)) # sleep for initial delay seconds
        for i in range(int(servo_multipler)):
            kit.servo[5].angle = servo_angle # go to specified angle channel 0
            sleep(servo_delay)
            kit.servo[5].angle = 0 # go to 0 angle channel 0
    elif servo_name == "07":
        sleep(int(servo_initial_delay)) # sleep for initial delay seconds
        for i in range(int(servo_multipler)):
            kit.servo[6].angle = servo_angle # go to specified angle channel 0
            sleep(servo_delay)
            kit.servo[6].angle = 0 # go to 0 angle channel 0
    elif servo_name == "08":
        sleep(int(servo_initial_delay)) # sleep for initial delay seconds
        for i in range(int(servo_multipler)):
            kit.servo[7].angle = servo_angle # go to specified angle channel 0
            sleep(servo_delay)
            kit.servo[7].angle = 0 # go to 0 angle channel 0
    elif servo_name == "09":
        sleep(int(servo_initial_delay)) # sleep for initial delay seconds
        for i in range(int(servo_multipler)):
            kit.servo[8].angle = servo_angle # go to specified angle channel 0
            sleep(servo_delay)
            kit.servo[8].angle = 0 # go to 0 angle channel 0
    elif servo_name == "10":
        sleep(int(servo_initial_delay)) # sleep for initial delay seconds
        for i in range(int(servo_multipler)):
            kit.servo[9].angle = servo_angle # go to specified angle channel 0
            sleep(servo_delay)
            kit.servo[9].angle = 0 # go to 0 angle channel 0
    elif servo_name == "11":
        sleep(int(servo_initial_delay)) # sleep for initial delay seconds
        for i in range(int(servo_multipler)):
            kit.servo[10].angle = servo_angle # go to specified angle channel 0
            sleep(servo_delay)
            kit.servo[10].angle = 0 # go to 0 angle channel 0
    elif servo_name == "12":
        sleep(int(servo_initial_delay)) # sleep for initial delay seconds
        for i in range(int(servo_multipler)):
            kit.servo[11].angle = servo_angle # go to specified angle channel 0
            sleep(servo_delay)
            kit.servo[11].angle = 0 # go to 0 angle channel 0
    elif servo_name == "13":
        sleep(int(servo_initial_delay)) # sleep for initial delay seconds
        for i in range(int(servo_multipler)):
            kit.servo[12].angle = servo_angle # go to specified angle channel 0
            sleep(servo_delay)
            kit.servo[12].angle = 0 # go to 0 angle channel 0
    elif servo_name == "14":
        sleep(int(servo_initial_delay)) # sleep for initial delay seconds
        for i in range(int(servo_multipler)):
            kit.servo[13].angle = servo_angle # go to specified angle channel 0
            sleep(servo_delay)
            kit.servo[13].angle = 0 # go to 0 angle channel 0
    elif servo_name == "15":
        sleep(int(servo_initial_delay)) # sleep for initial delay seconds
        for i in range(int(servo_multipler)):
            kit.servo[14].angle = servo_angle # go to specified angle channel 0
            sleep(servo_delay)
            kit.servo[14].angle = 0 # go to 0 angle channel 0
    elif servo_name == "16":
        sleep(int(servo_initial_delay)) # sleep for initial delay seconds
        for i in range(int(servo_multipler)):
            kit.servo[15].angle = servo_angle # go to specified angle channel 0
            sleep(servo_delay)
            kit.servo[15].angle = 0 # go to 0 angle channel 0
'''

def setStep(w1, w2, w3, w4): # Function for step sequence for Stepper Motor
    pass
    # GPIO.output(coil_A_1_pin, w1)
    # GPIO.output(coil_A_2_pin, w2)
    # GPIO.output(coil_B_1_pin, w3)
    # GPIO.output(coil_B_2_pin, w4)
'''
def run_stepper(code):
    print("Starting Stepper: "+ str(code))
    print ("%s" % ( time.ctime(time.time())))
    stepper_initial_delay = code[0:2]
    print("Stepper: Initial Delay", stepper_initial_delay)
    stepper_name = code[2:4]
    print("Stepper Name: ", stepper_name)
    stepper_rotation_clockwise = code[5:7]
    print("Stepper: Clockwise Rotation : ", stepper_rotation_clockwise)
    stepper_hold_seconds = code[8:10]
    print("Stepper: Hold seconds: ", stepper_hold_seconds)
    stepper_rotation_counter_clockwise = code[10:12]
    print("Stepper: Counter Clockwise Rotation: ", stepper_rotation_counter_clockwise)
    Stepper_repeat = code[14]
    print("Stepper: Repeat: ", Stepper_repeat)
    if stepper_name == "01": # Black(A+), Green(A-), Red(B+), Blue(B-)
        delay = 0.0055
        steps = 200
        sleep(int(stepper_initial_delay)) # sleep for initial delay
        for i in range(int(Stepper_repeat)):
            for j in range(0, steps): # Run the setps for clockwise
                setStep(1,0,1,0)
                time.sleep(delay)
                setStep(0,1,1,0)
                time.sleep(delay)
                setStep(0,1,0,1)
                time.sleep(delay)
                setStep(1,0,0,1)
                time.sleep(delay)
            sleep(stepper_hold_seconds) # Hold before counter clockwise
            for j in range(0, steps): # Reverse previous step sequence to reverse motor direction
                setStep(1,0,0,1)
                time.sleep(delay)
                setStep(0,1,0,1)
                time.sleep(delay)
                setStep(0,1,1,0)
                time.sleep(delay)
                setStep(1,0,1,0)
                time.sleep(delay)
    elif stepper_name == "02":
        print("No pins defined")
    elif stepper_name == "03":
        print("No pins defined")
'''
'''
def run_relay(code):
    print("Starting Relay: "+ str(code))
    print ("%s" % ( time.ctime(time.time())))
    relay_initial_delay = code[0:2]
    print("Relay: Initial Delay", relay_initial_delay)
    relay_name = code[2:4]
    print("Relay Name: ", relay_name)
    relay_seconds = code[4:6]
    print("Relay Seconnds: ", relay_seconds)
    relay_minutes = code[7:9]
    print("Relay Minutes: ", relay_minutes)
    relay_seconds = int(relay_seconds) + int(relay_minutes)*60
    if relay_name == "01":
        sleep(int(relay_initial_delay)) # sleep for initial delay
        pin0.value = False
        sleep(relay_seconds)
        pin0.value = True
    elif relay_name == "02":
        sleep(int(relay_initial_delay)) # sleep for initial delay
        pin1.value = False
        sleep(relay_seconds)
        pin1.value = True
    elif relay_name == "03":
        sleep(int(relay_initial_delay)) # sleep for initial delay
        pin2.value = False
        sleep(relay_seconds)
        pin2.value = True
    elif relay_name == "04":
        sleep(int(relay_initial_delay)) # sleep for initial delay
        pin3.value = False
        sleep(relay_seconds)
        pin3.value = True
    elif relay_name == "05":
        sleep(int(relay_initial_delay)) # sleep for initial delay
        pin4.value = False
        sleep(relay_seconds)
        pin4.value = True
    elif relay_name == "06":
        sleep(int(relay_initial_delay)) # sleep for initial delay
        pin5.value = False
        sleep(relay_seconds)
        pin5.value = True
    elif relay_name == "07":
        sleep(int(relay_initial_delay)) # sleep for initial delay
        pin6.value = False
        sleep(relay_seconds)
        pin6.value = True
    elif relay_name == "08":
        sleep(int(relay_initial_delay)) # sleep for initial delay
        pin7.value = False
        sleep(relay_seconds)
        pin7.value = True
    elif relay_name == "09":
        sleep(int(relay_initial_delay)) # sleep for initial delay
        pin8.value = False
        sleep(relay_seconds)
        pin8.value = True
    elif relay_name == "10":
        sleep(int(relay_initial_delay)) # sleep for initial delay
        pin9.value = False
        sleep(relay_seconds)
        pin9.value = True
    elif relay_name == "11":
        sleep(int(relay_initial_delay)) # sleep for initial delay
        pin10.value = False
        sleep(relay_seconds)
        pin10.value = True
    elif relay_name == "12":
        sleep(int(relay_initial_delay)) # sleep for initial delay
        pin11.value = False
        sleep(relay_seconds)
        pin11.value = True
    elif relay_name == "13":
        sleep(int(relay_initial_delay)) # sleep for initial delay
        pin12.value = False
        sleep(relay_seconds)
        pin12.value = True
    elif relay_name == "14":
        sleep(int(relay_initial_delay)) # sleep for initial delay
        pin13.value = False
        sleep(relay_seconds)
        pin13.value = True
    elif relay_name == "15":
        sleep(int(relay_initial_delay)) # sleep for initial delay
        pin14.value = False
        sleep(relay_seconds)
        pin14.value = True
    elif relay_name == "16":
        sleep(int(relay_initial_delay)) # sleep for initial delay
        pin15.value = False
        sleep(relay_seconds)
        pin15.value = True
'''
def run_motor(data):
    print("data found: ", data)
    string = data.replace(" ", "")
    list  = string.split(",")
    threads = []
    # Create new threads
    for i in range(len(list)):
        thread = myThread(i, "Thread-"+str(i), list[i])
        threads.append(thread)
    # Start new Threads
    for i in range(len(list)):
        threads[i].start()
    # join threads
    for t in threads:
        t.join()
    print ("Exiting Main Thread")

    return

while True:
    _, img = cap.read()
    data, bbox, _ = detector.detectAndDecode(img)

    if(bbox is not None):
        for i in range(len(bbox)):
            cv2.line(img, tuple(bbox[i][0]), tuple(bbox[(i+1) % len(bbox)][0]), color=(255,
                     0, 255), thickness=2)
        cv2.putText(img, data, (int(bbox[0][0][0]), int(bbox[0][0][1]) - 10), cv2.FONT_HERSHEY_SIMPLEX,
                    0.5, (0, 255, 0), 2)
        if data:
            print(data)
            end=time.time()
            if((end-start)>3 or prevdata!=data):
                run_motor(data)
                start = time.time()
            prevdata=data
    cv2.imshow("code detector", img)
    if(cv2.waitKey(1) == ord("q")):
        break
cap.release()
cv2.destroyAllWindows()
GPIO.cleanup()
'''
data = "000102r00,000902r00,001602r00"
sleep(1)
print("data found: ", data)
string = data.replace(" ", "")
list  = string.split(",")
threads = []
# Create new threads
for i in range(len(list)):
    thread = myThread(i, "Thread-"+str(i), list[i])
    threads.append(thread)
# Start new Threads
for i in range(len(list)):
    threads[i].start()
# join threads
for t in threads:
    t.join()
print ("Exiting Main Thread")
'''