Fixed some bugs in Synch code

The synch code should be working now (at least with 1 mote in the
network). Have not done extensive tests though so there might be more
bugs. Also cleaned up and moved around more code to make it easier to
look through.