Make algorithm to control actuators based on sonar ping results

Combine the motor driving modules with the controlled sonar modules so the results of the sonar pings will issue instructions to the motor modules to control the device.

Specific scenarios:

Obstacle Direction	Action
Top	Stop and Go Down
Bottom	Stop and Go Up
Left	Stop and Turn Right
Right	Stop and Turn Left
Front	Reverse

RichardSefton / AUV

Make algorithm to control actuators based on sonar ping results #9