Fixed everything and code is working right. Will upload schematics after launch