Add cpp library code for sensor and motor control
Add sensorOff detection