Add in sweet new motor power settings
Fix fp_EQ and fp_NE
More corner cases in planning, update the waypoints when we update mr