A Bio-Inspired Algorithm and Foldable Robot Platform for Collective Excavation