#importation des modules
import RPi.GPIO as GPIO
import time
import sys,tty,termios
import tkinter as tk
import threading                #librairie pour les threads, taches en parallele
from threading import Thread    #librairie pour les threads, taches en parallele
from queue import Queue

#initialisation de la numerotation
GPIO.setmode(GPIO.BOARD)

#Temps d'atente
global SPSTime 
global PSStime
SPSTime=0.02 #Max frequency : 25Mhz = 0.000 000 04s  0.0000002            #Serial Parallel
PSStime=0.01 #Max frequency : 50Mhz = 0.000 000 02s  0.0000001            #Parallel Serial


#Speed sensor variables
    #Leg 0, Motor 0
global SpSensL0M1
global SpSensL0M2

global SpSensL1M1
global SpSensL1M2

global SpSensL2M1
global SpSensL2M2

global SpSensL3M1
global SpSensL3M2


SpSensL0M1 = 0
SpSensL0M2 = 0

SpSensL1M1 = 0
SpSensL1M2 = 0

SpSensL2M1 = 0
SpSensL2M2 = 0

SpSensL3M1 = 0
SpSensL3M2 = 0

#Variables d'entrée

global MotorSelect #Selection of the motor to act on
MotorSelect=0


#Tableau de données d'entrée et sortie

DataLoadIn = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]; #Tableau des données d'entrée
DataLoadOut = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]; #Tableau des données de sortie


#XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

#Ser -> //
SPSER=40                                                        #Serial output pin
SPSCLK=36                                                       #Shift registers clock pin
SPRCLK=38                                                       #Output register clock

#// -> Ser

PSSIN=33                                                        #Serial data input
PSPL=35                                                         #Parallel loading of the inputs of the shift register
PSCLK=37

#Configuration de la commande du registre a décalage Paralle -> Serie GPIO
GPIO.setup(PSPL, GPIO.OUT, initial=True)                   # broche SER est une sortie numerique, a létat bas au début
GPIO.setup(PSSIN, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)          # broche SIN est une entrée avec resistance de pull down
GPIO.setup(PSCLK, GPIO.OUT, initial=False)

#Configuration de la commande du registre a décalage Serie -> Paralle GPIO
GPIO.setup(SPSER, GPIO.OUT, initial=False)                   # broche SER est une sortie numerique, a létat bas au début et avec resistance de pull down
GPIO.setup(SPSCLK, GPIO.OUT, initial=False)                  # broche SCLK est une sortie numerique
GPIO.setup(SPRCLK, GPIO.OUT, initial=False)

#XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

#Value to display on the screen through threading
global ThingToPrint
ThingToPrint = "char"

#Threader for displaying some data
class PrintScreen(Thread):
    
    def __init__(self):
        Thread.__init__(self)
        
    def WriteScreen():
        global ThingToPrint
        #print("Lis \n")
        while True:
            #global Arg
            print(ThingToPrint)
            time.sleep(1)
            
#XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

#Initialisation
GPIO.cleanup
print("clean up")
#Mise a 0 de toutes les sorties du registre a décalage
for SPiR in range(16):
    print("Reset", SPiR)
    GPIO.output(SPSER, False)
    #time.sleep(0.0001)
    GPIO.output(SPSCLK, True)
    #time.sleep(0.0001)
    GPIO.output(SPSCLK, False)
    #print("Reset=", SPiR)
#activation sortie registres
GPIO.output(SPRCLK, True)
#time.sleep(0.0001)
GPIO.output(SPRCLK, False)

#XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

#Function for writing data in the outputs S --> P
class Writing(threading.Thread):
    #S -> // thread

    def __init__(self):
        threading.Thread.__init__(self)

    #Continuously Write the data to the register
    def Write():
        
        global SPSTime
                
        while True:
            #print("debut while")
    #Serial output
            #DataLoadOut = [DataLoadOut[0], DataLoadOut[1], DataLoadOut[2], DataLoadOut[3], DataLoadOut[4], DataLoadOut[5], DataLoadOut[6], DataLoadOut[7], DataLoadOut[8], DataLoadOut[9], DataLoadOut[10], DataLoadOut[11], DataLoadOut[12], DataLoadOut[13], DataLoadOut[14], DataLoadOut[15]]; #Tableau des données de sortie
    #Sending data 1 by 1 starting from the last one to the register
            for SPiDL in reversed(DataLoadOut):
                if GPIO.input(SPSER) != SPiDL:#If the GPIO outpu pin is not in the wanted state, invert it
                    GPIO.output(SPSER, not GPIO.input(SPSER))
    #One clock cycle
                GPIO.output(SPSCLK, True) 
                GPIO.output(SPSCLK, False)
    #Activate shift register once to load the next data (no clock needed)
            GPIO.output(SPRCLK, True)
            GPIO.output(SPRCLK, False)
            #print("DataLoadOut", DataLoadOut)
            time.sleep(0.05)
            
#XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

#Reading the data from the shift register 
#// -> S thread
class Reading(threading.Thread):
    def __init__(self):
        threading.Thread.__init__(self)
        
    def Read():
        #Serial input
        global DataLoadIn
        global ThingToPrint
        print ("start P-S Thread")
        while True:
    #Loading data in register
            #Clock cycle on PS for loading data (no need of clock signal)
            GPIO.output(PSPL, False)
            GPIO.output(PSPL, True)
            
    #As long as the 16 bits didn't pass
            for i, PSiDL in enumerate(reversed(DataLoadIn)):
                DataIn = GPIO.input(PSSIN) #Reading the seril input
                DataLoadIn[15-i] = DataIn #Passing the read value in the table
                GPIO.output(PSCLK, True)#un coups d'horloge pour passer au bit suivant
                GPIO.output(PSCLK, False)
                #print("DataLoadIn", DataLoadIn)
    #Loading of the data in the table
            ThingToPrint = DataLoadIn


#XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

class TouchSensors(threading.Thread):
    def __init__(self):
        threading.Thread.__init__(self)
        
    def Sensors():
        #Serial input
        global DataLoadIn
        global DataLoadOut

        print ("start Sensors Thread")
        while True:
            #Leg 0, Right Sensor
            if DataLoadIn[2] == 1:
                DataLoadOut[0]=0
                DataLoadOut[1]=0
            #Leg 0, Left Sensor
            if DataLoadIn[3] == 1:
                DataLoadOut[2]=0
                DataLoadOut[3]=0
            #Leg 1, Right Sensor
            if DataLoadIn[6] == 1:
                DataLoadOut[4]=0
                DataLoadOut[5]=0
            #Leg 1, Left Sensor
            if DataLoadIn[7] == 1:
                DataLoadOut[6]=0
                DataLoadOut[7]=0
            #Leg 2, Right Sensor
            if DataLoadIn[10] == 1:
                DataLoadOut[8]=0
                DataLoadOut[9]=0
            #Leg 2, Left Sensor
            if DataLoadIn[11] == 1:
                DataLoadOut[10]=0
                DataLoadOut[11]=0
            #Leg 3, Right Sensor
            if DataLoadIn[14] == 1:
                DataLoadOut[12]=0
                DataLoadOut[13]=0
            #Leg 3, Left Sensor
            if DataLoadIn[15] == 1:
                DataLoadOut[14]=0
                DataLoadOut[15]=0

#XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
                
##class Moving(threading.Thread):
##    def __init__(self):
##        threading.Thread.__init__(self)
##        
##    def Movement():
##        DataLoadOut
##        DataLoadIn
##        while True:
##            if DataLoadIn[2] == 0:
                
            
        #Serial input


                
##            #Leg 1, Right Sensor
##            elif DataLoadIn[3] == 1
##                DataLoadOut[0]=0
##                DataLoadOut[1]=0
##            #Leg 0, Left Sensor
##            elif DataLoadIn[3] == 1
##                DataLoadOut[0]=0
##                DataLoadOut[1]=0
##            #Leg 1, Right Sensor
##            elif DataLoadIn[3] == 1
##                DataLoadOut[0]=0
##                DataLoadOut[1]=0
##            #Leg 0, Left Sensor
##            elif DataLoadIn[3] == 1
##                DataLoadOut[0]=0
##                DataLoadOut[1]=0
            #Motor 1, Right Sensor
            


#XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
#changing of the data for moving the legs
            
#Changes the data of the register to move the leg to the Right
def MRight():
    global DataLoadOut
    
    if DataLoadOut[0]==0 and DataLoadOut[1]==0 and DataLoadOut[4]==0 or (DataLoadOut[0]==1 and DataLoadOut[1]==1 and DataLoadOut[4]==1): #If stopped, start turning
        print("Problem, reset")
        DataLoadOut[0]=0
        DataLoadOut[1]=1
        DataLoadOut[4]=0
    if DataLoadOut[0]==0 and DataLoadOut[1]==1 and DataLoadOut[4]==0: #If stopped, start turning
        DataLoadOut[0]=0
        DataLoadOut[1]=1
        DataLoadOut[4]=1
    elif DataLoadOut[0]==1 and DataLoadOut[1]==0 and DataLoadOut[4]==1: #If turning the other way, stop it
        DataLoadOut[0]=0
        DataLoadOut[1]=1
        DataLoadOut[4]=0
    elif DataLoadOut[0]==1 and DataLoadOut[1]==0 and DataLoadOut[4]==0: #If turning the other way, stop it
        DataLoadOut[0]=0
        DataLoadOut[1]=1
        DataLoadOut[4]=1
    #DataLoadOut = [DataLoadOut[0], DataLoadOut[1], DataLoadOut[2], DataLoadOut[3], DataLoadOut[4], DataLoadOut[5], DataLoadOut[6], DataLoadOut[7], DataLoadOut[8], DataLoadOut[9], DataLoadOut[10], DataLoadOut[11], DataLoadOut[12], DataLoadOut[13], DataLoadOut[14], DataLoadOut[15]]; #Tableau des données de sortie



#Changes the data of the register to move the leg to the Left
def MLeft():
    global DataLoadOut
    if DataLoadOut[0]==0 and DataLoadOut[1]==0 and DataLoadOut[4]==0 or (DataLoadOut[0]==1 and DataLoadOut[1]==1 and DataLoadOut[4]==1): #If stopped, start turning
        print("Problem, reset")
        DataLoadOut[0]=0
        DataLoadOut[1]=1
        DataLoadOut[4]=0
    if DataLoadOut[0]==1 and DataLoadOut[1]==0 and DataLoadOut[4]==0: #If stopped, start turning
        DataLoadOut[0]=1
        DataLoadOut[1]=0
        DataLoadOut[4]=1
    elif DataLoadOut[0]==0 and DataLoadOut[1]==1 and DataLoadOut[4]==1: #If turning the other way, stop it
        DataLoadOut[0]=1
        DataLoadOut[1]=0
        DataLoadOut[4]=0
    elif DataLoadOut[0]==0 and DataLoadOut[1]==1 and DataLoadOut[4]==0: #If turning the other way, stop it
        DataLoadOut[0]=1
        DataLoadOut[1]=0
        DataLoadOut[4]=1
    #DataLoadOut = [DataLoadOut[0], DataLoadOut[1], DataLoadOut[2], DataLoadOut[3], DataLoadOut[4], DataLoadOut[5], DataLoadOut[6], DataLoadOut[7], DataLoadOut[8], DataLoadOut[9], DataLoadOut[10], DataLoadOut[11], DataLoadOut[12], DataLoadOut[13], DataLoadOut[14], DataLoadOut[15]]; #Tableau des données de sortie



#Changes the data of the register to move the leg to the Left
def MFwd():
    global DataLoadOut
    global MotorSelect

#Leg 0 Motor 1
    if MotorSelect == 1:
        if DataLoadOut[0]==1 and DataLoadOut[1]==1: #If Bug, Stop turning
            print("Problem, reset")
            DataLoadOut[0]=0
            DataLoadOut[1]=0
        if DataLoadOut[0]==0 and DataLoadOut[1]==0: #If stopped, start turning
            DataLoadOut[0]=1
            DataLoadOut[1]=0
        elif DataLoadOut[0]==0 and DataLoadOut[1]==1: #If turning the other way, stop it
            DataLoadOut[0]=0
            DataLoadOut[1]=0
#Leg 0 Motor 2
    elif MotorSelect==2:
        if DataLoadOut[2]==1 and DataLoadOut[3]==1: #If Bug, Stop turning
            print("Problem, reset")
            DataLoadOut[2]=0
            DataLoadOut[3]=0
        if DataLoadOut[2]==0 and DataLoadOut[3]==0: #If stopped, start turning
            DataLoadOut[2]=1
            DataLoadOut[3]=0
        elif DataLoadOut[2]==0 and DataLoadOut[3]==1: #If turning the other way, stop it
            DataLoadOut[2]=0
            DataLoadOut[3]=0
#Leg 1 Motor 1
    elif MotorSelect==3:
        if DataLoadOut[4]==1 and DataLoadOut[5]==1: #If Bug, Stop turning
            print("Problem, reset")
            DataLoadOut[4]=0
            DataLoadOut[5]=0
        if DataLoadOut[4]==0 and DataLoadOut[5]==0: #If stopped, start turning
            DataLoadOut[4]=1
            DataLoadOut[5]=0
        elif DataLoadOut[4]==0 and DataLoadOut[5]==1: #If turning the other way, stop it
            DataLoadOut[4]=0
            DataLoadOut[5]=0
#Leg 1 Motor 2
    elif MotorSelect==4:
        if DataLoadOut[6]==1 and DataLoadOut[7]==1: #If Bug, Stop turning
            print("Problem, reset")
            DataLoadOut[6]=0
            DataLoadOut[7]=0
        if DataLoadOut[6]==0 and DataLoadOut[7]==0: #If stopped, start turning
            DataLoadOut[6]=1
            DataLoadOut[7]=0
        elif DataLoadOut[6]==0 and DataLoadOut[7]==1: #If turning the other way, stop it
            DataLoadOut[6]=0
            DataLoadOut[7]=0
#Leg 2 Motor 1
    if MotorSelect == 5:
        if DataLoadOut[8]==1 and DataLoadOut[9]==1: #If Bug, Stop turning
            print("Problem, reset")
            DataLoadOut[8]=0
            DataLoadOut[9]=0
        if DataLoadOut[8]==0 and DataLoadOut[9]==0: #If stopped, start turning
            DataLoadOut[8]=1
            DataLoadOut[9]=0
        elif DataLoadOut[8]==0 and DataLoadOut[9]==1: #If turning the other way, stop it
            DataLoadOut[8]=0
            DataLoadOut[9]=0
#Leg 2 Motor 2
    elif MotorSelect==6:
        if DataLoadOut[10]==1 and DataLoadOut[11]==1: #If Bug, Stop turning
            print("Problem, reset")
            DataLoadOut[10]=0
            DataLoadOut[11]=0
        if DataLoadOut[10]==0 and DataLoadOut[11]==0: #If stopped, start turning
            DataLoadOut[10]=1
            DataLoadOut[11]=0
        elif DataLoadOut[10]==0 and DataLoadOut[11]==1: #If turning the other way, stop it
            DataLoadOut[10]=0
            DataLoadOut[11]=0
#Leg 3 Motor 1
    elif MotorSelect==7:
        if DataLoadOut[12]==1 and DataLoadOut[13]==1: #If Bug, Stop turning
            print("Problem, reset")
            DataLoadOut[12]=0
            DataLoadOut[13]=0
        if DataLoadOut[12]==0 and DataLoadOut[13]==0: #If stopped, start turning
            DataLoadOut[12]=1
            DataLoadOut[13]=0
        elif DataLoadOut[12]==0 and DataLoadOut[13]==1: #If turning the other way, stop it
            DataLoadOut[12]=0
            DataLoadOut[13]=0
#Leg 3 Motor 2
    elif MotorSelect==8:
        if DataLoadOut[14]==1 and DataLoadOut[15]==1: #If Bug, Stop turning
            print("Problem, reset")
            DataLoadOut[14]=0
            DataLoadOut[15]=0
        if DataLoadOut[14]==0 and DataLoadOut[15]==0: #If stopped, start turning
            DataLoadOut[14]=1
            DataLoadOut[15]=0
        elif DataLoadOut[14]==0 and DataLoadOut[15]==1: #If turning the other way, stop it
            DataLoadOut[14]=0
            DataLoadOut[15]=0
    
    #DataLoadOut = [DataLoadOut[0], DataLoadOut[1], DataLoadOut[2], DataLoadOut[3], DataLoadOut[4], DataLoadOut[5], DataLoadOut[6], DataLoadOut[7], DataLoadOut[8], DataLoadOut[9], DataLoadOut[10], DataLoadOut[11], DataLoadOut[12], DataLoadOut[13], DataLoadOut[14], DataLoadOut[15]]; #Tableau des données de sortie
    #print(DataLoadOut)


#Changes the data of the register to walk backward
def MBwd():
    global DataLoadOut
    global MotorSelect
    if MotorSelect == 1:
        if DataLoadOut[0]==1 and DataLoadOut[1]==1: #If Bug, Stop turning
            print("Problem, reset")
            DataLoadOut[0]=0
            DataLoadOut[1]=0
        if DataLoadOut[0]==0 and DataLoadOut[1]==0: #If stopped, start turning
            DataLoadOut[0]=0
            DataLoadOut[1]=1
        elif DataLoadOut[0]==1 and DataLoadOut[1]==0: #If turning the other way, stop it
            DataLoadOut[0]=0
            DataLoadOut[1]=0
            
    elif MotorSelect==2:
        if DataLoadOut[2]==1 and DataLoadOut[3]==1: #If Bug, Stop turning
            print("Problem, reset")
            DataLoadOut[2]=0
            DataLoadOut[3]=0
        if DataLoadOut[2]==0 and DataLoadOut[3]==0: #If stopped, start turning
            DataLoadOut[2]=0
            DataLoadOut[3]=1
        elif DataLoadOut[2]==1 and DataLoadOut[3]==0: #If turning the other way, stop it
            DataLoadOut[2]=0
            DataLoadOut[3]=0
            
    elif MotorSelect==3:
        if DataLoadOut[4]==1 and DataLoadOut[5]==1: #If Bug, Stop turning
            print("Problem, reset")
            DataLoadOut[4]=0
            DataLoadOut[5]=0
        if DataLoadOut[4]==0 and DataLoadOut[5]==0: #If stopped, start turning
            DataLoadOut[4]=0
            DataLoadOut[5]=1
        elif DataLoadOut[4]==1 and DataLoadOut[5]==0: #If turning the other way, stop it
            DataLoadOut[4]=0
            DataLoadOut[5]=0
            
    elif MotorSelect==4:
        if DataLoadOut[6]==1 and DataLoadOut[7]==1: #If Bug, Stop turning
            print("Problem, reset")
            DataLoadOut[6]=0
            DataLoadOut[7]=0
        if DataLoadOut[6]==0 and DataLoadOut[7]==0: #If stopped, start turning
            DataLoadOut[6]=0
            DataLoadOut[7]=1
        elif DataLoadOut[6]==1 and DataLoadOut[7]==0: #If turning the other way, stop it
            DataLoadOut[6]=0
            DataLoadOut[7]=0
            
    if MotorSelect == 5:
        if DataLoadOut[8]==1 and DataLoadOut[9]==1: #If Bug, Stop turning
            print("Problem, reset")
            DataLoadOut[8]=0
            DataLoadOut[9]=0
        if DataLoadOut[8]==0 and DataLoadOut[9]==0: #If stopped, start turning
            DataLoadOut[8]=0
            DataLoadOut[9]=1
        elif DataLoadOut[8]==1 and DataLoadOut[9]==0: #If turning the other way, stop it
            DataLoadOut[8]=0
            DataLoadOut[9]=0
            
    elif MotorSelect==6:
        if DataLoadOut[10]==1 and DataLoadOut[11]==1: #If Bug, Stop turning
            print("Problem, reset")
            DataLoadOut[10]=0
            DataLoadOut[11]=0
        if DataLoadOut[10]==0 and DataLoadOut[11]==0: #If stopped, start turning
            DataLoadOut[10]=0
            DataLoadOut[11]=1
        elif DataLoadOut[10]==1 and DataLoadOut[11]==0: #If turning the other way, stop it
            DataLoadOut[10]=0
            DataLoadOut[11]=0
            
    elif MotorSelect==7:
        if DataLoadOut[12]==1 and DataLoadOut[13]==1: #If Bug, Stop turning
            print("Problem, reset")
            DataLoadOut[12]=0
            DataLoadOut[13]=0
        if DataLoadOut[12]==0 and DataLoadOut[13]==0: #If stopped, start turning
            DataLoadOut[12]=0
            DataLoadOut[13]=1
        elif DataLoadOut[12]==1 and DataLoadOut[13]==0: #If turning the other way, stop it
            DataLoadOut[12]=0
            DataLoadOut[13]=0
            
    elif MotorSelect==8:
        if DataLoadOut[14]==1 and DataLoadOut[15]==1: #If Bug, Stop turning
            print("Problem, reset")
            DataLoadOut[14]=0
            DataLoadOut[15]=0
        if DataLoadOut[14]==0 and DataLoadOut[15]==0: #If stopped, start turning
            DataLoadOut[14]=0
            DataLoadOut[15]=1
        elif DataLoadOut[14]==1 and DataLoadOut[15]==0: #If turning the other way, stop it
            DataLoadOut[14]=0
            DataLoadOut[15]=0
    #DataLoadOut = [DataLoadOut[0], DataLoadOut[1], DataLoadOut[2], DataLoadOut[3], DataLoadOut[4], DataLoadOut[5], DataLoadOut[6], DataLoadOut[7], DataLoadOut[8], DataLoadOut[9], DataLoadOut[10], DataLoadOut[11], DataLoadOut[12], DataLoadOut[13], DataLoadOut[14], DataLoadOut[15]]; #Tableau des données de sortie
    #print(DataLoadOut)

        
#Detection of the key press events
def key(event):
    global MotorSelect
    print("press " + event.keysym)
    if event.keysym == "Right":
        MRight()
    elif event.keysym == "Left":
        MLeft()
    elif event.keysym == "Up":
        MFwd()
    elif event.keysym == "Down": 
        MBwd()
    elif event.keysym == "KP_1":
        MotorSelect=1
    elif event.keysym == "KP_2":
        MotorSelect=2
    elif event.keysym == "KP_3":
        MotorSelect=3
    elif event.keysym == "KP_4":
        MotorSelect=4
    elif event.keysym == "KP_5":
        MotorSelect=5
    elif event.keysym == "KP_6":
        MotorSelect=6
    elif event.keysym == "KP_7":
        MotorSelect=7
    elif event.keysym == "KP_8":
        MotorSelect=8
    elif event.keysym == "Escape": 
        StopProgram()
        
#XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX


#Class for calculating the frequency of the Speed sensors
class SpSens(threading.Thread):
    #Speed recording Thread
        def __init__(self):
            threading.Thread.__init__(self)
            
        def SpSensL0M0():
            global DataLoadIn
            global ThingToPrint
            
            global SpSensL0M1
            global SpSensL0M2

            global SpSensL1M1
            global SpSensL1M2

            global SpSensL2M1
            global SpSensL2M2

            global SpSensL3M1
            global SpSensL3M2
            
            #1 value out of X are displayed
            OneOn=10

            #Variables to store the last state of the GPIO Inputs
            LastDataL0M1=0
            LastDataL0M2=0

            LastDataL1M1=0
            LastDataL1M2=0

            LastDataL2M1=0
            LastDataL2M2=0

            LastDataL3M1=0
            LastDataL3M2=0

            #Timers for calculating the frequency of the signals
            EndTimeL0M1=0
            StartTimeL0M1=0
            EndTimeL0M1=0
            StartTimeL0M1=0

            EndTimeL0M1=0
            StartTimeL0M1=0
            EndTimeL0M1=0
            StartTimeL0M1=0

            EndTimeL0M1=0
            StartTimeL0M1=0
            EndTimeL0M1=0
            StartTimeL0M1=0

            EndTimeL0M1=0
            StartTimeL0M1=0
            EndTimeL0M1=0
            StartTimeL0M1=0

            LastRiseL0M1=0
            print("Start SpSens Thread")
            while True:
                #print("DataLoad In = ", DataLoadIn[0], DataLoadIn[1], DataLoadIn[2], DataLoadIn[3], )
                #print("DataLoadIn[0] = ", DataLoadIn[0])
                StartTime=time.time()               #Record the start reading time
                if LastDataL0M1==0 and DataLoadIn[0]==1:      #if the last data read was a 1
                    RiseL0M1=time.time()            #Record the time when the bit went to 1
                    LastDataL0M1=1                  #Store the State that was just read
                    Impulsion = RiseL0M1-LastRiseL0M1 #calculate the time passed between the last record and the current one
                    if OneOn==0:                    #Changes the data to print only once on 10
                        ThingToPrint = DataLoadIn[0], DataLoadIn[1], DataLoadIn[2], DataLoadIn[3]
                        OneOn=10                    #Put the counter back to it's original value
                    LastRiseL0M1=RiseL0M1               #Store the actualy read time for the next pass
                    OneOn-=1                        #Decrement the reading variable
                elif LastDataL0M1==1 and DataLoadIn[0]==0:    #If the last data read was a 0
                    LastDataL0M1=0
                    #print("Fall")
                #time.sleep(0.01)
                    
                #else:

#XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

#Create the threads
CallPSThread = threading.Thread(target=Reading.Read)
print("Create Call PS Thread")
CallSPThread = threading.Thread(target=Writing.Write)
print("Create Call SP Thread")
CallSpSensL0M0 = threading.Thread(target=SpSens.SpSensL0M0)
print("Create Call Sensors Thread")


#Start the threads
CallPSThread.start()
print("Start PS Thread")
CallSPThread.start()
print("Start the Serial -> Parallel Thread")
CallSpSensL0M0.start()
print("Start Sensors Thread")

# Thread for displaying something at screen
CallPrintScreen = threading.Thread(target=PrintScreen.WriteScreen)
print("call Print\n")
CallPrintScreen.start()
print("Call Print Start\n")

#Create a TK Window
root = tk.Tk()
print("Reading Keyboard inputs")
root.bind_all('<Key>', key)













#XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX

def StopProgram():
    global DataLoadOut
    DataLoadOut[0]=0 #In1
    DataLoadOut[1]=0 #In2
    DataLoadOut[2]=0 #In3
    DataLoadOut[3]=0 #In4
    DataLoadOut[4]=0 #ENA
    DataLoadOut[5]=0 #ENB
    #DataLoadOut = [DataLoadOut[0], DataLoadOut[1], DataLoadOut[2], DataLoadOut[3], DataLoadOut[4], DataLoadOut[5], DataLoadOut[6], DataLoadOut[7], DataLoadOut[8], DataLoadOut[9], DataLoadOut[10], DataLoadOut[11], DataLoadOut[12], DataLoadOut[13], DataLoadOut[14], DataLoadOut[15]]; #Tableau des données de sortie
    CallSpThread.join()

    print("Stop the Serial -> Parallel Thread")
    CallPSThread.stop()
    print("Stop PS Thread")
    CallSPThread.stop()
    print("Stop Sensors Thread")
    CallSpSensL0M0.stop()
    print("Stop Print Start\n")
    CallPrintScreen.stop()
    print("Stop Speed sensor L0M0")
    
    CallPSThread.join()
    CallSpSensL0M0.join()
    CallPrintScreen.join()
    CallSpSensL0M0.join()

    sys.exit()