Coordinate and angle lookup working also began testing with sonar