Matlab Code For Robot Control