Introduction

The purpose of the calculation is to determine if an object detected by the sensor is inside one of the grid squares to be mapped. The sensor can be located anywhere as long as:

• The sensor's coordinates are known
• The sensor's X axis is parallel to the grid's X axis (If not, see the information below)
• Both the sensor's and grid's positive X and Y axes point in the same direction

The sensor's X axis is the servo's zero (0) direction. If the servo's zero (0) direction is not parallel to the grid's X axis (this is the usual case), see the servo information on how to make adjustments.

The sensor angle is measured counterclockwise from the grid's positive X axis. This makes the sine and cosine of the angle have the correct sign (+,-) for the coordinate calculations. Note: There is information about calculating the sensor angle on the servo page.

Once an object's coordinates are known, it is easy to determine if the object falls within the grid to be mapped and which cell in the grid the object is in. Note: the size of the grid and the size of each cell in the grid must be known.

The servo's zero (0) direction is the result of setting the servo's position to zero (0). The sensor is mounted on the servo so that this is also the sensor's positive X axis. It does not have to be, but if it is not there are extra calculations required.

Diagram

Calculations (pseudo code)

OX - object x coordinate
OY - object y coordinate
SX - sensor x coordinate
SY - sensor y coordinate
SA - sensor angle
D  - distance from sensor to object

OX = SX + (cos(SA) * D)
OY = SY + (sin(SA) * D)

GMINX - grid minimum x coordinate
GMINY - grid minimum y coordinate
GMAXX - grid maximum x coordinate
GMAXY - grid maximum y coordinate

if ((OX < GMINX) or (OX > GMAXX) or
    (OY < GMINY) or (OY > GMAXY))
{
  print "The object is outside of the grid."
}

CX  - cell X size
CY  - cell Y size
COL - object cell column
ROW - object cell row

COL = int((OX)/CX)
ROW = int((OY)/CY)

PERL Code

use strict;
use Math::Trig;

# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# do not call this subroutine if the distance is zero (0)
# zero (0) means no object was detected
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

sub objCoordCalc
{
   my $sx = $_[0];             # sensor X coord
   my $sy = $_[1];             # sensor Y coord
   my $sa = $_[2];             # sensor angle (degrees)
   my $d  = $_[3];             # sensor to object distance     

   my $r = $sa * (pi/180.0);   # sensor angle in radians

   my $x = 0;                  # object X coordinate
   my $y = 0;                  # object Y coordinate

   $x = $sx + (cos($r) * $d); 
   $y = $sy + (sin($r) * $d);

   return ($x,$y);
}