Code for the waspmotes