Distance scanner using servomotor and distance sensor

In my previous post we got a servomotor doing what we want, so now it is time to put it to use! We will mount a Sharp GP2YOA distance sensor on top of the servo, so we can measure the distance to obstacles at different angles. We can then mount this sensor-servo combination on a robot to give it a good overview of the environment!

The hot glue gun!

I read a lot about robotic projects online, and I have several books about the topic. The coolest tool present in all of those is without competition the hot glue gun .. A few months ago I was in a hobby store and came across one by accident so I bought one. Though it is not as spectacular as I had hoped for, it certainly is really handy to glue things together like lego and sensors. 

First I glued a Lego piece on the sensor:

The hot glue gun, the servo and the lego

The image below shows how the component is glued to the sensor. Note the piece of cardboard to protect the electronics from the glue (though I don’t think it would hurt the electronics, I didn’t want to take the risk :p)

We will also glue a piece of Lego to the servo:

 

Jop, it's a bit messy

Now we can just plug the sensor Lego piece into the servo Lego piece, et voilà!

 

This already looks a bit like a robot..

The code

The hard bits are already done in past projects:

• Controlling the servo motor
• Reading in sensor values and sending them to Matlab

So now it is just a matter of putting the two together.. I’ve put the code in a file called scanner.c. The scanStep() function will move the sensor one step (in this example 2°) and measure the distance. It will then send the current angle and distance value on the COM port using the format D,[angle],[distance]. 

#include "scanner.h"
#define SCAN_RESOLUTION 2	//measure distance every 2°
short servoDir		= 0;	//0 = clockwise / 1=ccw
int distances[180/SCAN_RESOLUTION+1];
/**
 * Moves the servo from left to right and back (one step per call)
 **/
void moveServo() {
	static short servoAngle	= 90;
	short oldAngle = servoAngle;
	if (servoDir == 0) {			//clockwise
		servoAngle+=SCAN_RESOLUTION;
		if (servoAngle>=180) {
			servoDir = 1;
			//servoAngle = 0;	
		}
	} else if (servoDir == 1) {		//counterclockwise
		servoAngle-=SCAN_RESOLUTION;
		if (servoAngle<=0) {
			servoDir = 0;
		}
	}
	setServoAngle(servoAngle);
	//give servo some time to reach goal angle
	if (oldAngle>servoAngle) {
		Wait_Ms(20*(oldAngle-servoAngle));
	} else {
		Wait_Ms(20*(servoAngle-oldAngle));
	}
}
void scanStep() {
	int distance = 0;
	int angle = 0;
	moveServo();						//move servo 5° further
	angle = getServoAngle();
	distance = readLightSensor(0);		//read distance at this angle
	distances[angle/SCAN_RESOLUTION+1] = distance;
	fprintf (_H_USART, "D,%i,%i\n",angle,distances[angle/SCAN_RESOLUTION+1]);
}

Now we want to plot these values in Matlab. This is done using the following code. First we open the COM port and wait for data to arrive. We then put the data in an array B, where the position in B determines the angle the measurement was made at. We then plot these values so our plot resembles the directions the measurements where made at.

function [B] = sensorPlot2()
COMPORT             = 4;
SCAN_RESOLUTION     = 2;    %make sure this matches SCAN_RESOLUTION in embedded code!

%open com port
s2 = serial(['COM' num2str(COMPORT)],'BaudRate',19200);
fopen(s2);
s2.ReadAsyncMode = 'continuous';

%open plot window
colordef none
h=figure('Color',[0.3 0.3 0.3]);

%init values
quit=0;
paused=0;
B = zeros(180/SCAN_RESOLUTION+1,1);
theAngles = (1:length(B))*SCAN_RESOLUTION*pi/180; %we will plot values on these angles

try
    while 1
        while ~s2.BytesAvailable    %wait for samples to become available
            pause(0.02);
        end
        if s2.BytesAvailable
            try
                res1 = dataFromResult(fscanf(s2)); %read the angle and distance value into res1 = [angle, distanceValue]
                res = res1(2);    resAngle = res1(1);
            catch
                res=[];
                display('something went wrong!');
            end
            samplesRead = size(res,1);
            if samplesRead>0
                indexB = resAngle/SCAN_RESOLUTION+1;
                if indexB>0 && indexB<=180/SCAN_RESOLUTION+1
                    B(round(indexB)) = res;
                    plotVals = [(1000-B).*cos(theAngles') (1000-B).*sin(theAngles')];
                    plot(plotVals(:,1),plotVals(:,2),'-.');
                    hold on;
                    plot(plotVals(round(indexB),1),plotVals(round(indexB),2),'-gO');
                    hold off;
                    xlim([-1000 1000]); ylim([-100 1000]);
                    pause(0.000002);    %so the plot is updated
                end
            end
        end
    end
catch ME1
    ME1.message,ME1.stack,ME1.cause,fclose(s2);delete(s2);
end
try
    fclose(s2);delete(s2);clear s2
end
colordef white;
%% converts the result to a numeric row(kabraeck)
    function [ data ] = dataFromResult( result )
        remain = result;
        index=1;
        tokens=[];
        while (~isempty(remain))
            [token,remain] = strtok(remain, ',');
            if (index>1)
                tokens(index-1)=strread(token);
            end
            index=index+1;
        end
        data = tokens;
    end
end

The image below shows an example plot Matlab generates:

The green dot indicates the current measurement

 You can see that there is an obstacle on the left of the robot. 

Below are two videos that are perhaps more illustrative:

Code download

You can download the complete code here. This is an Eclipse project (see my other article on how to use Eclipse for PIC development), but you can import the .c and .h files into MPlab if you like. Note that this project assumes to be loaded onto the PIC with an USB bootloader (see this article for details)!

Scope with PIC microcontroller and Matlab

After many trials I recently succeeded in using the serial connection for communication between my PIC experimental board (based on a PIC18F4455) and my laptop. Turned out I was configuring my UART wrong, but now it works. I’m very happy about this because it is very useful for debugging my robot controller. I wrote a script in Matlab to read out the sensorvalues from the COM port and plot them in some sort of scope-window. The result looks pretty cool!