Hi, @gingergod Welcome to the forum.
Your code and schematic would make this a whole lot easier.
Thanks.. Tom...