UPdated the control code and added ArduIMU code