Distance Calibration and Angle of Same

Ok, here are the current issues:

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:

Fiddling with a full setup on the Arduino

I decided to go ahead and put together a complete setup of software in the Arduino.  I have been developing the interfaces to the servos, ir distance sensor, ultrasonic distance sensor, and LCD serial panel.  In the course of the changes, I decided to move the read of the ir distance sensor to the Arduino rather than read it through the Maestro.  I may end up changing my mind later.  So my setup in the IDE looks something like this:

// Pololu Mini-Maestro 18 Serial Servo Controller
// Power 9v Seperate power supply from Arduino
// GND to GND on the Arduino
// SSC RX pin to Arduino TX pin Pin04
// SSC TX pin to Arduino RX pin Pin03
// Channel 0 - Ultrasonic Distance Sensor Servo
// Channel 2 - Camera Pan Servo
// Channel 4 - Camera Tilt Servo
// Channel 6 - IR Distance Sensor Servo

// GP2D12 IR Distance Sensor
// Power 5v from the Arduino
// GND to GND on the Arduino
// GP2D12 Sensor analog Rx pin to Arduino Analog pin 0

// Parallax 2x16 Serial LCD Panel
// Power 5v from the Arduino
// GND to GND on the Arduino
// LCD Rx pin to Arduino TX pin Pin06

// HC-SR04 Ultrasonic Distance Sensor
// Power 5v from the Arduino
// GND to GND on the Arduino
// HC-SR04 Trig to Arduino TX pin Pin13
// HC-SR04 Echo to Arduino RX pin Pin12

This setup ends up forcing a number of pin definitions as so:

// Define constants
// Serial pins
const int tx_ToSSC=4; ///< Tx pin going to SSC
const int rx_ToSSC=3; ///< Rx pin coming from SSC
const int tx_ToLCD=6; ///< Tx pin going to LCD panel
const int rx_FrmLCD=5; ///< Rx pin coming from LCD panel(nc)
// Digital pins
const int digtx_ToUltraTrig=12; ///< Tx pin going to ultrasonic sensor trig
const int digrx_FrmUltraEcho=13; ///< Rx pin coming from ultrasonic sensor echo
// Channels on SSC
const int chan_ultraservo=0; ///< Channel 0 - Ultrasonic Distance Sensor Servo
const int chan_camerapanservo=2; ///< Channel 2 - Camera Pan Servo
const int chan_cameratiltservo=4; ///< Channel 4 - Camera Tilt Servo
const int chan_irdistservo=6; ///< Channel 6 - IR Distance Sensor Servo
// Analog pins
const int rx_irdistanalog=0; ///< IR Distance Sensor analog Rx


I have gleaned from several sources the code that I need to access each of these items.  First up, is my interfaces to the two serial connections needed for the SSC (Maestro) and the LCD panel:

// initalizing serial connections
SoftwareSerial SSC_Serial = SoftwareSerial(rx_ToSSC, tx_ToSSC);
SoftwareSerial LCD_Serial = SoftwareSerial(rx_FrmLCD, tx_ToLCD);

Next up, I have the procedure interface to the SSC that sets a servos pwm value:

// Send a Set servo pwm command to the Maestro.
// Target is in units of quarter microseconds
// so the normal range is 4000 to 8000.
void ssc_cmnd_pwm(unsigned char servo, unsigned int target)
{
    SSC_Serial.write(0xAA); //start byte
    SSC_Serial.write(0x0C); //device id
    SSC_Serial.write(0x04); //command number
    SSC_Serial.write(servo); //servo number
    SSC_Serial.write(target & 0x7F);
    SSC_Serial.write((target >> 7) & 0x7F);
}

Next, I have a few routines for writing information out to the LCD panel:

// display two strings on the LCD panel
void lcd_display_2_str(String myvalue1, String myvalue2) {
  delay(100);
  LCD_Serial.write(12); // Clear            
  LCD_Serial.print(myvalue1); // First line
  LCD_Serial.write(13); // Form Feed
  LCD_Serial.print(myvalue2); // Second line
} 

// display string and int value on one line, string on second line on the LCD panel
void lcd_display_2_str_int(String myvalue1, int myvalue, String myvalue2) {
  delay(100);
  LCD_Serial.write(12); // Clear            
  LCD_Serial.print(myvalue1); // First line
  LCD_Serial.print(myvalue, DEC);
  LCD_Serial.write(13); // Form Feed
  LCD_Serial.print(myvalue2); // Second line
} 

// display integer value on the LCD panel
void lcd_display_int_val(int myvalue) {
  delay(100);
  LCD_Serial.write(12);                 // Clear            
  LCD_Serial.print("val: ");
  LCD_Serial.print(myvalue, DEC);  // First line
  LCD_Serial.write(13);                 // Form feed
} 

Next, I deal with the gp2d12 ir range detector (along with a test):

/*
 read_gp2d12_range
 Function that reads a value from GP2D12 infrared distance sensor and returns a value in centimeters.

 This sensor should be used with a refresh rate of 36ms or greater.

 BY: Javier Valencia 2008

 float read_gp2d12_range(byte pin)

 It can return -1 if something gone wrong.

 */

float read_gp2d12_range(byte pin) {
    int tmp;

    tmp = analogRead(pin);
    if (tmp < 3)
        return -1; // invalid value

    return (6787.0 /((float)tmp - 3.0)) - 4.0;
}
void test_irdistance()
{
  float myval;
  myval = read_gp2d12_range(rx_irdistanalog);
  Serial.print("ir val: ");
  Serial.println(myval);
//  LCD_Serial.write(12);                 // Clear            
//  LCD_Serial.print("ir val: ");
//  LCD_Serial.print(myval, DEC);         // First line
//  LCD_Serial.write(13);                 // Form feed
}

Next, I deal with the Ultrasonic distance sensor (HC-SR04):

/*
  HC-SR04 Ping distance sensor
  VCC to Arduino 5v, GND to Arduino GND
  Echo to Arduino pin 13, Trig to Arduino pin 12
  more info at: http://goo.gl/kJ8G1
  */

float grab_ultrasonic()
{
  int duration;
  float distance;
  digitalWrite(digtx_ToUltraTrig, LOW);
  delayMicroseconds(2);
  digitalWrite(digtx_ToUltraTrig, HIGH);
  delayMicroseconds(10);
  digitalWrite(digtx_ToUltraTrig, LOW);
  duration = pulseIn(digrx_FrmUltraEcho, HIGH);
  distance = (duration/2) / 29.1;
  return distance;
}

void test_ultrasonic()
{
  float mydist;
  mydist = grab_ultrasonic();
  if (mydist >= 200 || mydist <= 0){
    Serial.println("Ultra: Out of range");
//    LCD_Serial.write(12);                 // Clear            
//    LCD_Serial.print("Out of range");
//    LCD_Serial.write(13);                 // Form feed
  } else {
    Serial.print("Ultra: ");
    Serial.print(mydist);
    Serial.println(" cm");
//    LCD_Serial.write(12);                 // Clear            
//    LCD_Serial.print("Out of range");
//    LCD_Serial.print(mydist, DEC);
//    LCD_Serial.print(" cm");
//    LCD_Serial.write(13);                 // Form feed
  }
}

In order to utilize these, I have the following setups:

// run once, when the sketch starts
void setup()
{
  // setup pins for serial interface to SSC
  pinMode(rx_ToSSC, INPUT);
  digitalWrite(tx_ToSSC, HIGH);
  pinMode(tx_ToSSC, OUTPUT);
  // set SSC serial interface speed
  SSC_Serial.begin(9600);

  // setup pins for serial interface to LCD
  pinMode(rx_FrmLCD, INPUT);
  digitalWrite(tx_ToLCD, HIGH);
  pinMode(tx_ToLCD, OUTPUT);
  // set LCD serial interface speed
  LCD_Serial.begin(9600);
  // initial clear and backlight for LCD
  LCD_Serial.write(12); // Clear            
  LCD_Serial.write(17); // Turn backlight on
  delay(5);             // Required delay for response

  // set SSC servos to initial state
  ssc_cmnd_pwm(chan_ultraservo,6000);
  ssc_cmnd_pwm(chan_camerapanservo,6000);
  ssc_cmnd_pwm(chan_cameratiltservo,6000);
  ssc_cmnd_pwm(chan_irdistservo,6000);
 
  // set Ultrasonic Distance Sensor pins
  pinMode(digtx_ToUltraTrig, OUTPUT);
  pinMode(digrx_FrmUltraEcho, INPUT);
 
  // setup our serial monitor
  Serial.begin(9600);
 
  // wait for settling to happen
  delay(1000); 
}

Now that I have the initial software coded, I have time to try them out and start to build the interfaces between the Arduino and the sensors/servos.  Next up will be results of the testing, followed by dealing with the interface to the RPi.

Next Move - Get Arduino SW Working

Connections are in place and now I can settle down to figuring out the software in the Arduino, the breadboard connections for same, and the interface software from the RPi.  I still have a number of interfaces that need to be tested:

1. the Ultrasonic Distance sensor needs to be wired up through the breadboard and the interface software needs to be written for the Arduino
2. the IR Distance sensor interface software needs to be written for the Arduino
3. the interface software for the two distance sensor servos needs to be written for the Arduino
4. the interface software for the two camera servos needs to be written for the Arduino
5. a control protocol, including response, needs to be thought up between the RPi and the Arduino and coded on both sides - ultimately this will be used by several Facades in the RPi software
6. a control protocol, including response, needs to be thought up between the RPi and the iRobot Create and coded in the RPi
7. afterwards I can concentrate on more higher level software, including the blackboard.

The connection diagram has changed

Now that I have come to the conclusion that I can move a lot of the hardware control down to the Arduino, I have redone the Test Setup connection diagram.

This diagram now shows the two ways that I have of controlling the iCreate, either through the Arduino or via a USB serial line.  In addition, I have added the IR and Ultrasonic Distance Sensors and their servos.  I changed out the SSC-32 for the Maestro, since that seems to be a better bet.  I have also added a temporary place holder for a Joystick/Button shield connected to an Arduino.  The purpose for this connection is to be able to use a Joystick controller to control movement and camera remotely, not something I want to have in the final model.

Was able to Setup Full Connection

Now that I have settled in on what the interfaces should be, I took the time to mount the Arduino and the Pololu Micro-Maestro onto the Lexan piece.  I did take advantage of the fact that the Lexan is not conductive, and just screwed the Maestro down onto the plastic without any kind of standoffs.




A close up of the connections is shown in the following picture.

I will say that I did at first have problems with the setup.  After I got everything mounted I tried out the Arduino test program that I had written, and I could not get it to work.  I started removing all kinds of wires and eventually got everything down to just the interface to the LCD panel and I got it to work.  What I did not realize was that when I turned on the power to the servos, I failed to turn on the master switch on the power platform.  That is why the servos would not operate, they did not have any voltage because I had removed the voltage jumper on the Maestro.  Once I figured that problem out I was able to get both the one servo and the LCD to respond.  Maybe I need a checklist.

Got Servo and LCD Panel to Work Together

I was wondering if the little Arduino would be able to keep up with a multiple number of tasks that I was giving it. I was able to program the Arduino UNO from my RPi to have two serial connections. One was to my Pololu Mini-Maestro and the other was to my Parallax 2x16 LCD display. I was able to get it to display what values I was sending to the Maestro. I did discover that turning off the backlight on the LCD took too much time. I also discovered that the servos that I am using respond better to 9 volts than they do to 6 volts. The serial connections were at a 5 volt level.

On the Maestro, I used the gnd, 5 volt, Rx, and Tx lines on the side and connected them to gnd, 5 volts, digital pin 4, and digital pin 3. This is the GND, VIN, RX, and TX pins on the left hand side of the board.  I removed the VSRV=VIN jumper and connected the 9 volt supply to the servo power on the bottom right.  During the test I had the servo connected to channel 0.

On the LCD panel, I used the gnd, 5 volt, and Rx lines and connected them to gnd, 5 volt, and digital pin 6. Fortunately, I was able to easily accommodate the different pins due to the fact that I was using the Parallax breadboard shield which breaks out each of these pins around the breadboard.

My main motivation in doing this experiment was to see how many things I could seasonably  attach to the Arduino that would be controlled independently.  This gives me hope for being able to move much of the hardware control down to the Arduino micro-controller.

Starting Up Robot SW Build

Over the next few months I will be posting updates on what is going on with the Robot SW build.  I intend on building the software from a design at the top which will meet with some experimental software which drives different components at the bottom.  Hopefully, they will meet somewhere in the middle correctly.  The reason that I need to work on the bottom components is that I need to make sure that I have a sufficient set of facades that will talk to the hardware correctly.  I am excited about the prospects and from here on out will try to give a play by play description of what I am up to.  I will be using BoUML for the UML design and will document things by posting them here.