The excavate program included in the mod kind of does that already. Look into the zip file of CC, go to lua\rom\programs\turtle and open it. Learn from the code.
Another option would be to use GPS (save the chest coords and return to there, when your 16th slot starts to fill).