IR Scanner - Prototype Feedback

Projects General Guidance
1

Hoping to get some advice or direction on a project of mine.
The project goal is to create a point map using distance measurements taken from a sensor. I have created a prototype, but I am running into a few issues.

The prototype consists of:

  • Arduino Uno
  • XBee mounted on an XBee shield
  • Two Hi-tec HS-422 servos
  • PulsedLight LIDAR-Lite IR distance sensor

Nothing special in the physical setup of this, the servos have 100uf decoupling capacitors. Here is a picture:

This prototype communicates with an XBee that I have plugged into my laptop. I read the serial data in a Visual Studio created program, in which I then draw a basic depth image using OpenGL.

The logical process of the Arduino prototype is:

  • Scan horizontally right to left for 90 degrees, storing one measurement per degree
  • Serially send the 90 values (I convert the array to a String and send the string)
  • Move back to the rightmost position and vertically move down 2 degrees
  • Repeat 1 to 3 until 45 horizontal scans have been completed

This provides a final array on my laptop that is 90 by 45 (array[45][90]). I then iterate over this and draw each point as its measurement value divided by 255 to get a colour value. So 0 being black and 1 being white, a measurement of 150cm becomes 150/255 = 0.588.

Here is a video of the prototype in action:
Working IR Scan (Youtube)

Here is the OpenGL result, which doesn't look too bad (although I have just realised that the shape on the left, which I thought was some shelves is actually a squashed version of the main image):

Now here is a video of one of the issues that I am encountering. About 50% of the time, at any point during the scan, the servos will start to move incredibly slowly and with random jitters. Does anyone know what could be causing this?
IR Scan with issues (Youtube)

One issue I have already overcome was the conflict between SoftwareSerial and Servo due to the libraries using interrupts (I think?), which is why I am using SoftwareServo within my Arduino code.

I would greatly appreciate any feedback or ideas on how to improve this design, even if it means changing a fundamental part. Some of my current thoughts are:

I am a little concerned at the speed of the device; there is obviously a massive hit in speed as soon as I start taking the IR distance measurements. Is there anything I could actually do to help this, or is that just the limitation of the Arduino Uno?

The final 2D depth image isn’t particularly clear, but I suppose that is just because of the resolution and could be resolved with more measurements (At the hit of a much slower scan). Can I move a servo for decimal degrees (ie. move it 0.5 each time to get double the readings)?

2

Here is the Arduino code. Note that before it serially sends it waits for an "A" to be sent from the laptop:

#include <SoftwareServo.h>
#include <SoftwareSerial.h>
#include <I2C.h>
#define    LIDARLite_ADDRESS   0x62          // Default I2C Address of LIDAR-Lite.
#define    RegisterMeasure     0x00          // Register to write to initiate ranging.
#define    MeasureValue        0x04          // Value to initiate ranging.
#define    RegisterHighLowB    0x8f          // Register to get both High and Low bytes in 1 call.

SoftwareServo horizontalServo;
SoftwareServo verticalServo;

// XBee's DOUT (TX) is connected to pin 8 (Arduino's Software RX)
// XBee's DIN (RX) is connected to pin 9 (Arduino's Software TX)
SoftwareSerial softwareSerial(8, 9); // RX, TX

int distance[90];
int horizontalAngle;
int verticalAngle;
int horizontalLoopCount;
boolean firstSweep = true;

void setup() { 
  Serial.begin(9600); 
  softwareSerial.begin(9600);
  //LIDAR
  setupLIDAR();
  //Servo's
  horizontalServo.attach(2);
  verticalServo.attach(4);   
  horizontalAngle = 135;
  verticalAngle = 145;
  horizontalLoopCount = 0;
  horizontalServo.write(horizontalAngle);  
  delay(2);
  verticalServo.write(verticalAngle);
  delay(500);
}

void loop() {
  SoftwareServo::refresh();
  horizontalServo.write(horizontalAngle);  
  delay(2);
  verticalServo.write(verticalAngle);
  delay(2);

  //Take measurement and move servo
  addReading();
  horizontalAngle--;
  
  //Vertical Servo
  if(verticalAngle == 55 ){
    verticalAngle = 145;
    verticalServo.write(verticalAngle);
    horizontalAngle = 135;
    horizontalServo.write(horizontalAngle);    
    delay(1000);
  }
  
  //If end of sweep, reset horizontal angle, decrement verical angle and send readings
  if(!firstSweep && (/*horizontalAngle == 135 ||*/ horizontalAngle == 45 )){
    horizontalAngle = 135;
    verticalAngle -= 2;
    horizontalLoopCount = 0;
    sendReadings();
    horizontalServo.write(horizontalAngle);  
    verticalServo.write(verticalAngle);
    delay(20);
  }
  else if(firstSweep){
    firstSweep = false;
  }
}

//Adds a reading to the array and increments the loop counter
void addReading(){
  if(horizontalLoopCount < 90){
    distance[horizontalLoopCount] = llGetDistance();
    horizontalLoopCount++;
  }
}

//Creates a string of readings from the array.
//Then converts string to a char array to be serially sent.
void sendReadings(){
  String distanceReadingsString;
  for(int i = 0; i<90; i++){
    if(i<89){
      distanceReadingsString+=distance[i];
      distanceReadingsString+=",";
    }
    else{
      distanceReadingsString+=distance[i];
      distanceReadingsString+="~";
    }
  }
  
  //+1 to null terminate char array
  char charArray[distanceReadingsString.length()+1];
  distanceReadingsString.toCharArray(charArray, distanceReadingsString.length()+1);
  Serial.println(charArray);
 

  boolean dataSent = false;
  while (!dataSent){
    //check for received serial data
    while (softwareSerial.available()){
 char read = char(softwareSerial.read());
        //If read data is 'A' send readings
 if (read == 'A'){
  softwareSerial.write(charArray);
          dataSent = true;
 }
        break; 
    }  
  }
}


//********LIDAR CODE******

void setupLIDAR(){    
  I2c.begin(); // Opens & joins the irc bus as master
  delay(100); // Waits to make sure everything is powered up before sending or receiving data  
  I2c.timeOut(50); // Sets a timeout to ensure no locking up of sketch if I2C communication fails
}

// Write a register and wait until it responds with success
void llWriteAndWait(char myAddress, char myValue){
 uint8_t nackack = 100; // Setup variable to hold ACK/NACK resopnses     
 while (nackack != 0){ // While NACK keep going (i.e. continue polling until sucess message (ACK) is received )
 nackack = I2c.write(LIDARLite_ADDRESS, myAddress, myValue); // Write to LIDAR-Lite Address with Value
 delay(2); // Wait 2 ms to prevent overpolling
 }
}

// Read 1-2 bytes from a register and wait until it responds with sucess
byte llReadAndWait(char myAddress, int numOfBytes, byte arrayToSave[2]){
 uint8_t nackack = 100; // Setup variable to hold ACK/NACK resopnses     
 while (nackack != 0){ // While NACK keep going (i.e. continue polling until sucess message (ACK) is received )
 nackack = I2c.read(LIDARLite_ADDRESS, myAddress, numOfBytes, arrayToSave); // Read 1-2 Bytes from LIDAR-Lite Address and store in array
 delay(2); // Wait 2 ms to prevent overpolling
 }
 return arrayToSave[2]; // Return array for use in other functions
}

//Get 2-byte distance from sensor and combine into a single 16 bit int
int llGetDistance(){
 llWriteAndWait(0x00, 0x04); // Write 0x04 to register 0x00 to start getting distance readings
 byte myArray[2]; // array to store bytes from read function
 llReadAndWait(0x8f, 2, myArray); // Read 2 bytes from 0x8f
 int distance = (myArray[0] << 8) + myArray[1];  // Shift high byte [0] 8 to the left and add low byte [1] to create 16-bit int
 return(distance);
}

//Get average distance reading
double llGetDistanceAverage(int numberOfReadings){
 if (numberOfReadings < 2){
 numberOfReadings = 2; // If the number of readings to be taken is less than 2, default to 2 readings
 }
 double sum = 0; // Variable to store sum
 for (int i = 0; i < numberOfReadings; i++){
 sum = sum + llGetDistance(); // Add up all of the readings
 }
 sum = sum / numberOfReadings; // Divide the total by the number of readings to get the average
 return(sum);
}

If you would like to see any snippets of the OpenGL code just ask, but all it does is read in the measurements serially and store them in an array. It then draws this array of measurements as GL_POINTS in a 90 by 45 grid as shown in the example image above.

Thanks in advance for any help or advice that anyone can give.

3

The servo jitters are probably caused by SoftwareSerial. The SoftwareSerial library disables interrupts for the duration of each character received or sent. At 9600 bits per second a character takes about a millisecond to send. That's 1000 microseconds, enough to send a servo off the end of it's normal range. I recommend switching to an Arduino Leonardo so you can use the built-in USB (Serial) for debug and the hardware serial (Serial1) for the Xbee.

4

As John said, or maybe an Arduino Mega will do as it has 4x hardware serial ports.
Maybe speeding up serial baud from 9600 to 115200 will speed things along a bit.

5

Ok, I am relatively new to Arduino's, so apologies if this question sounds silly. So what you are saying is that the Leonardo and Mega have a means of controlling the servos and the XBee with their own individual timers?

Also, is the mega faster? As in would the device overall process and move at a faster rate?

6

calco:
So what you are saying is that the Leonardo and Mega have a means of controlling the servos and the XBee with their own individual timers?

No. The Leonardo and MEGA have spare hardware serial ports so you don't need to use SoftwareSerial which is interfering with timing.

calco:
Also, is the mega faster? As in would the device overall process and move at a faster rate?

No, the MEGA is the same speed. It has more memory and more I/O pins.

7

Another option might be to try AltSoftSerial if the required pins are available (or can be changed). You can define what timer it uses so maybe careful selection of timer and servo pin might help prevent the jitters.

8

Can I move a servo for decimal degrees (ie. move it 0.5 each time to get double the readings)?

You can get more resolution in servo output, assuming that the servo is mechanically capable of it, by using the writeMicroseconds() method rather than the write() method when using the standard Servo library, but I don't know whether the softwareServo library supports it.

9

Hi,
Its not clear in the picture, but how are you powering everything, including the servos and IR unit?

Can you please post a copy of your circuit, in CAD or a picture of a hand drawn circuit in jpg, png?

Tom.... :slight_smile:

10

Haven't drawn up a circuit yet, all powered via Arduino Uno USB. Got a 4 x AA battery pack in the post so I can power the servo's from that when it arrives.

Ah ok John, I'll test out what Riva said about altSoftSerial and see if that sorts the issue. If it does I can use the standard servo library with writeMicroseconds as suggested by Bob. Otherwise it looks like I might have to do some more shopping!

Would SoftwareSerial still mess with the servos even after sending? Because the bit in the video where the servos are moving incredibly slow is when there is no serial data being sent/received?

11

Plotting in 3D now, still looking into the jitter issue :confused:
But, without doing any vector maths to correct for the single point of scan, here is the progress so far:
IR Scan in 3D (Youtube)
Still got the strange little duplicate image on the left, but I'll figure that out on a rainy day.

12

Right, I have tried altSoftSerial. Sadly that's not going to work because I am using servos.

Now I have also discovered that the slow movement issue shown in the video at the top actually has nothing to do with serial comms. It is something to do with my LIDAR lite or software servo code. But I'll leave that be for the time being.

I need to be using the standard servo library as SoftwareServo doesn't have a writeMicroseconds() method, so I only have 0-180 degrees of movement, which isn't enough. So I am looking at getting a Arduino Mega for the hardware serial ports.
I have only used softwareSerial with the XBee, so is it straightforward to use hardware serial with it?
I have this XBee Shield, with jumpers on D8 and D9 for RX and TX.

Also as a side question, the site says that that shield is compatible with Mega, but I can't see how it would fit on it?

Thanks for any help anyone can give!

13

calco:
I need to be using the standard servo library as SoftwareServo doesn't have a writeMicroseconds() method, so I only have 0-180 degrees of movement, which isn't enough.

I wonder if a servo is the right tool for the job if your wanting less than 1 degree movement accuracy.