adjusted diagnosticDrive function
testing navigationLogicC with PID library
various improvements and testing