Force Calculations

getNextPoint in CartController now calculates some forces