Updated with sensor code (almost working..)