Added basic code based on full robot