Program now succesfully connects to robot