Some of the constrains on a WSN are the limited power source this kind of nodes rely on, and the unreliable wireless links, hence it is imperative to implement different self-healing and topology control mechanisms to extend the lifetime of the network. We proposed and implemented a layered algorithm that generates the initial topology of the network, splits the network in clusters, selects a Cluster Head (CH) that represents the cluster, and puts to sleep the Non-CHs nodes (preserving their battery), and finally implements a mechanism that creates disjoint paths to recover the network when a single node fails. The whole system is implemented on real nodes running Contiki OS and Matlab on the sink.