HIL Testing

Hil testing for new servo scaled code