commit with partially working mathematical function in drawLine function inside Robot.cpp. Does not fully work, arm is getting stuck for unknown reason.