def turnLeft(self):
        print("##################TurnLeft##################")
        turnSpeed = self.turnSpeeds
        self.read_all()

##        imp.reload(HBridge)
        self.timeout = time.time()+4
        now = time.time()

        K = 0.98
        K1 = 1 - K

        time_diff = 0.01

##        (gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z, mag_scaled_x, mag_scaled_y, mag_scaled_z) = self.read_all()
        (gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z) = self.read_all()

        last_x = self.get_x_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
        last_y = self.get_y_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
        last_z = self.get_z_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)

        gyro_offset_x = gyro_scaled_x
        gyro_offset_y = gyro_scaled_y
        gyro_offset_z = gyro_scaled_z

        gyro_total_x = (last_x) - gyro_offset_x
        gyro_total_y = (last_y) - gyro_offset_y
##        gyro_total_z = (last_z) - gyro_offset_z
        gyro_total_z = 0
        if self.initialDegree == 999:
            self.initialDegree = (last_z) - gyro_offset_z


        self.rightTrigger =  0.5 -90
        self.leftTrigger = - 0.5 -90
##        self.rightTrigger = gyro_total_z + 1.5
##        self.leftTrigger = gyro_total_z  - 1.5

#Debug        print("LeftTrigger: " + str(self.leftTrigger) + " Actual: " + str(gyro_total_z) + " RightTrigger: " + str(self.rightTrigger))

        for i in range(0, int(3.0 / time_diff)):
            time.sleep(time_diff - 0.005)
            if time.time()>self.timeout:
                break

##            (gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z, mag_scaled_x, mag_scaled_y, mag_scaled_z) = self.read_all()
            (gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z) = self.read_all()

            gyro_scaled_x -= gyro_offset_x
            gyro_scaled_y -= gyro_offset_y
            gyro_scaled_z -= gyro_offset_z

            gyro_x_delta = (gyro_scaled_x * time_diff)
            gyro_y_delta = (gyro_scaled_y * time_diff)
            gyro_z_delta = (gyro_scaled_z * time_diff)

            gyro_total_x += gyro_x_delta
            gyro_total_y += gyro_y_delta
            gyro_total_z += gyro_z_delta
#Debug            print("LeftTrigger: " + str(self.leftTrigger) + " GyroTotalZ: " + str(gyro_total_z) + " RightTrigger: " + str(self.rightTrigger))
            #Ha balra ter ki
            if self.leftTrigger > gyro_total_z:
#Debug                print("<-------------balra ter ki")

                HBridge.setMotorLeft(-turnSpeed)
                #akkor a jobb motornak lassitani kell
                HBridge.setMotorRight(turnSpeed)
##                time.sleep(0.1)
##                HBridge.setMotorLeft(0)
##                HBridge.setMotorRight(0)

            #Ha jobbra ter ki
            elif self.rightTrigger < gyro_total_z:

#Debug                print("jobbra ter ki---------------->")
                #akkor a bal motornak lassitani kell
                HBridge.setMotorLeft(turnSpeed)
                HBridge.setMotorRight(-turnSpeed)
##                time.sleep(0.1)
##                HBridge.setMotorLeft(0)
##                HBridge.setMotorRight(0)

            else:
            #Kulonben azonos sebesseggel megy mind2 motor
#Debug                print("Furdulas kesz")
                HBridge.setMotorLeft(0)
                HBridge.setMotorRight(0)

        HBridge.setMotorLeft(0)
        HBridge.setMotorRight(0)

    def turnRight(self):
        print("##################TurnRight##################")
        self.read_all()
        turnSpeed = self.turnSpeeds
        ##imp.reload(HBridge)
        self.timeout = time.time()+4
        now = time.time()

        K = 0.98
        K1 = 1 - K

        time_diff = 0.01

##        (gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z, mag_scaled_x, mag_scaled_y, mag_scaled_z) = self.read_all()
        (gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z) = self.read_all()

        last_x = self.get_x_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
        last_y = self.get_y_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
        last_z = self.get_z_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)

        gyro_offset_x = gyro_scaled_x
        gyro_offset_y = gyro_scaled_y
        gyro_offset_z = gyro_scaled_z

        gyro_total_x = (last_x) - gyro_offset_x
        gyro_total_y = (last_y) - gyro_offset_y
##        gyro_total_z = (last_z) - gyro_offset_z
        gyro_total_z = 0
        if self.initialDegree == 999:
            self.initialDegree = (last_z) - gyro_offset_z


        self.rightTrigger =  0.5 +90
        self.leftTrigger = - 0.5 +90
##        self.rightTrigger = gyro_total_z + 1.5
##        self.leftTrigger = gyro_total_z  - 1.5

#Debug        print("LeftTrigger: " + str(self.leftTrigger) + " Actual: " + str(gyro_total_z) + " RightTrigger: " + str(self.rightTrigger))

        for i in range(0, int(3.0 / time_diff)):
            time.sleep(time_diff - 0.005)
            if time.time()>self.timeout:
                break

##            (gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z, mag_scaled_x, mag_scaled_y, mag_scaled_z) = self.read_all()
            (gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z) = self.read_all()

            gyro_scaled_x -= gyro_offset_x
            gyro_scaled_y -= gyro_offset_y
            gyro_scaled_z -= gyro_offset_z

            gyro_x_delta = (gyro_scaled_x * time_diff)
            gyro_y_delta = (gyro_scaled_y * time_diff)
            gyro_z_delta = (gyro_scaled_z * time_diff)

            gyro_total_x += gyro_x_delta
            gyro_total_y += gyro_y_delta
            gyro_total_z += gyro_z_delta
#Debug            print("LeftTrigger: " + str(self.leftTrigger) + " GyroTotalZ: " + str(gyro_total_z) + " RightTrigger: " + str(self.rightTrigger))

            #Ha balra ter ki
            if self.leftTrigger > gyro_total_z:
#Debug                print("<-------------balra ter ki")

                HBridge.setMotorLeft(-turnSpeed)
                #akkor a jobb motornak lassitani kell
                HBridge.setMotorRight(turnSpeed)

            #Ha jobbra ter ki
            elif self.rightTrigger < gyro_total_z:

#Debug                print("jobbra ter ki---------------->")
                #akkor a bal motornak lassitani kell
                HBridge.setMotorLeft(turnSpeed)
                HBridge.setMotorRight(-turnSpeed)

            else:
            #Kulonben azonos sebesseggel megy mind2 motor
#Debug                print("Furdulas kesz")
                HBridge.setMotorLeft(0)
                HBridge.setMotorRight(0)