need expression for robotics (to tell the space between objects)

[mr roboto]

hi all

i need some help with somthing for my robotics hobby.

heres the problem:

 

i have a robot. it has a scanning distance sensor.(sharp ir gp2 20-150 cm to those who are interested.)

now what i want to do is i need an expression to tell me if a gap is large enough to fit through. the example demonstrates this. i have the sharp ir mounted on a servo (motor)

the sharp ir turns. you have acess to the following pieces of info:

 

variables: distance1 (you can take this at anytime the sharp is rotating. i can code it to take it at the edge of the wall.)

 

distance2: same as distance 1

angle 1: taken at the same time as distance 1. it is the angel of the servo at that time. the angle will affect the distance of the sharp ir. the light has to take a longer path

angle 2: same as angle 1 but taken at the same time as distance 2

 

constants:

 

robot width: the robot is 3 inches wide but the gap needs to be 4.5 inches wide.

robot length: 5 inches

placement of scanner: in the middle width wise. also the sensor is at the back. the sensor will be place at the very back but the end of the sensor extends about 1.5 inches so it is taking readings from 1.5 inches.

 

if you need more info just ask.

[khaled]

Take in account that readings are from the sensor tip, not the robot's body center

 

for small spaces, you can approximate distance using Euclidian distance formula: [math]d \approx \sqrt{(x_1 - x_2)^2 + (y_1 - y_2)^2}[/math]

 

For large distances, you will need the real value, using Haversine formula: [math]d = 2r \arcsin{(\sqrt{\sin^2{(\frac{\phi_2 - \phi_1}{2})} + \cos{(\phi_1)} \cos{(\phi_2)} \sin^2{(\frac{\psi_2 - \psi_1}{2})}})}[/math]

 

where [math]\phi[/math] is the latitude, and [math]\psi[/math] is the longitude

Edited January 8, 2012 by khaled