1. I have two distance sensors on the Lexan plate. One is a GP2D12 ir distance sensor that is supposed to measure some short range distances. The other is a HC-SR04 ultrasonic distance sensor that measures somewhat larger distances. Each of these distance sensors have non-linearity in the values they return.
2. Each of the distance sensors are mounted on servos on either side of the Lexan plate, meaning that the center of rotation is off center from the iRobot Create.
3. When the servos were mounted, I noticed that they were not completely perpendicular to the Lexan plate, nor is the Lexan plate oriented in a perpendicular manner when mounted to the iRobot Create. That means that the angle of rotation needs to be adjusted. In addition, the Lexan plate is somewhat offset from the center of the iRobot Create and may have a rotation effect.
4. There is an area on the plate where each of the distance sensors can rotate to and an area where they cannot, a sector of a circle if you will. There will need to be adjustments to the sweep for each of the distance sensors so that the values they return will be acceptable.
5. Servos have issues in that there are discrete steps that they make rather than a continuous movement. Also, there needs to be a correlation between the pwm value and the angle of rotation for the servos, which may be different in each servo.
The above issues make it difficult to relate the measured values with what is really out there. This post is an attempt to define those issues and point to a solution. The overall requirement is to have measurements be absolute to a specific point, in this case the centroid of the iRobot Create. In order to do this I will have to deal with a number of coordinate systems. To simplify everything, I am going to assume an XY plane (i.e., no Z axis involved here), however I will retain XYZ coordinates for purposes of being able to add them in later. In this case translation is only in the XY plane, rotation is only about the Z axis, and the Z value is always 0. Note that I will assume no scaling. I will use matrix algebra to translate everything into iRobot Create coordinates (assuming that 0,0 is at the center of the Create). So when I take a measurement of a point, it can be translated into the iRobot Create coordinates. So to make it easier we have rotation around the Z axis at an angle psi with translation in the X (Tx) and Y (Ty). Note that normally the translation in Z (Tz) would be zero.
Notice that if there is no rotation, i.e. psi is zero, then cos psi is 1.0 and sin psi is 0.0 which is just a standard identity matrix with the translation parameters imposed. Where to begin:
a. Assume center of Lexan plate is the Lexan plate origin. The centroid of the plate should be an easy calculation, assume normal XY coordinates - measurements should be consistent (i.e., in inches).
b. Assume center of servo rotation is the servo origin. Distance measurements from each distance sensor should be to the servo origin. This simplifies the remaining transformations.
c. Servo -> Lexan coordinates: translate servo origin to Lexan center, rotate about Lexan center to compensate for Servo angle offset, scaling values should be equal to one (no scaling). It should be easy to measure the servo origins in relation to the plate and figure out the translation values. The servo angle offset may be more difficult, in the initial for simplification we can assume 0 degrees.
d. Lexan -> Create coordinates: translate Lexan origin to iRobot Create origin to compensate for plate offset, rotate about iRobot Create origin to compensate for plate rotation offset, scaling values should be equal to one (no scaling).
e. Create -> World coordinates: translate Create origin to World origin, rotate about World origin to get final World coordinates correct. No scaling, probably set the first matrix to an identity matrix for no effect.
Following the normal convention for such things, the final form of the equation is:
