Rewrite with custom struct and multithreading

driveController task, robotDrive struct