Changed defines to uppercase from camelCase
Implemented simple program code

If a perfect world, the robot would work now.