Added footnormal function (tries to) generate CoG from 4 FSRs