Moved code to bcm2835 lib for gpio control