E-Puck Color Sensing Project

e-Puck with the Zigbee module add-on board and red color sensor.

Overview

Project by: James Yeung, Electrical Engineering, Class of 2009.
Last updated: June 16, 2008

The goal of this project was to have the e-Puck roll around on the floor under a projected image, take sensor readings with one of the R G or B sensors, send back the position and color data to a listening PC by RS232 protocol via bluetooth, and occasionally receive from the PC new (x,y,theta) data on the actual position of the e-Puck (between receiving this data, the robot keeps track of its position by dead reckoning; upon receiving the data, the robot resets its estimated position to the new data). The robot had to roll around to cover the whole projected image (which is unchanging), and at the end of the run, the host PC had to display the reconstructed image and plot the error from the actual image. On the host PC side, MATLAB was used as an interface to talk to the e-Puck and generate the appropriate graphs.

Circuitry For Color Sensor

The photo diodes that I worked with were from Hamamatsu. These were the ones that were considered.

Circuit diagram for attempt 1.

• S9702 - RGB sensor (1mm^2 active area) - ~$9 (min 10)
• S9032-2 - RGB sensor (2mm^2 active area) - ~$15 (min 5)
• S6430-01 - Red color sensor (4mm^2 active area) - ~$11 (min 10)
• S6429-01 - Green color sensor (4mm^2 active area) - ~$11 (min 10)
• S6428-01 - Blue color sensor (4mm^2 active area) - ~$11 (min 10)

I mainly worked with the S9702 and the S6400 series sensors.

Attempt 1

I started by simply hooking the sensors with the circuit to the right. It works fairly well for the S6400 series sensors when measuring voltage with a multimeter or oscilloscope. But for the S9702, the responsiveness was not as great and so higher resistor values were needed to amplify the signal. However, at such high resistances, it was over the input impedance of my measuring devices, so my measurements were not valid. And even if the measuring devices had a higher input impedance, the input pin of the micro controller only has a 2.5K ohm input impedance.

An interesting find from this attempt was that for sensing ambient room light, the resistor values needed to be higher for green, and even higher for blue in order to get the full voltage swing of response.

Circuit diagram for attempt 2.

Circuit diagram for attempt 3.

Attempt 2

To combat the high impedance of the output, I tried to use an op-amp as a voltage follower. However, I got some strange periodic behavior in the voltage output and also some strange voltage off-set. It would read from -0.5V-ish to 2.7V-ish, a -0.5 off-set. Other more complicated circuitry were also recommended and tried by Prof. Lynch and Prof. Peshkin, but none really worked very well for me.

Attempt 3

This was the third and final attempted circuitry. I used a simple transistor to give the output roughly a 200x current gain, which allowed me to use a much smaller resistor to lower the output impedance. Although I did not look into this in great detail, it seems like there was a huge variance in the amount of current gain between individual transistors. Furthermore, different resistor values work better under different light conditions. So a trim-pot might be a better design if you plan on reproducing this circuit.

Code

Full source code

Overview

The code listed here will allow the e-Puck to sync up with the MATLAB program, calibrate the sensor, start the lawn mowing sweep, and update position/heading coordinates. Sensor readings along with the location are taken every 200ms and sent back to the PC only when the e-Puck is going in a straight line. Much of the code was programmed in the same fashion as the standard library of e-Puck codes and NUtest.

Code for e-Puck

This code borrows many functions from the standard e-Puck library and other added functions by Prof. Lynch in the NUtest.c version of the e-Puck code.

// main.c for RedScan project
// James Yeung
// 6/1/2008
//
// This code is for the e-puck to be used in conjunction with RedScan.m in MATLAB.  It will control the e-Puck to set up the bluetooth
// connection with PC, calibrate the sensor, perform the lawn mowing pattern, send sensor readings to PC, and allow user update of
// position/heading coordinates.

#include <p30f6014A.h>  					// contains register definitions, etc., for the dsPIC

#include <string.h>  						// string manipulation
#include <ctype.h>   						// contains the "toupper" command to convert lower to uppercase
#include <stdio.h>   						// standard I/O routines
#include <math.h>    						// math functions like sqrt, cos, etc.

#include <a_d/advance_ad_scan/e_ad_conv.h>  			// reads the A/D channelsp
#include <a_d/advance_ad_scan/e_acc.h>	    			// reads the 3 accelerometer channels (using e_ad_conv)
#include <motor_led/e_epuck_ports.h>        			// gives mnemonic names to pins, defines simple asm functions
#include <motor_led/e_init_port.h>          			// initializes the port values and whether input or output
#include <motor_led/advance_one_timer/e_led.h>  		// uses the LED's with interrupts, to allow blinking, etc.
#include <motor_led/advance_one_timer/e_motors_NU.h>  		// interrupt-driven motor routines (step every x millisecs, e.g.)
#include <motor_led/advance_one_timer/e_agenda_NU.h>  		// manages "agenda" (the interrupt routines)
#include <uart/e_uart_char.h>               			// uses the uarts for comm (in this case, bluetooth with PC)

#define uart_send_static_text(msg) do { e_send_uart1_char(msg,sizeof(msg)-1); while(e_uart1_sending()); } while(0)
#define uart_send_text(msg) do { e_send_uart1_char(msg,strlen(msg)); while(e_uart1_sending()); } while(0)
#define SENSOR_OFFSET 4.5

static char buffer[52*39*2+3+80];
int i;
char c='z';
float x,y,theta,dist,heading,diff,x_target,y_target,theta_target,speed,j;

// return the angle limited to [-PI, PI]; PI defined in e_motors_NU.h
float anglelimit(float ang) {
  while (ang<-PI) ang += 2.0*PI;
  while (ang>PI) ang -= 2.0*PI;
  return ang;
}

void go_to(float x_target, float y_target, float theta_target){
	theta_target = anglelimit(theta_target*DEG2RAD);
	e_get_configuration(&x,&y,&theta);
	dist = sqrt((x-x_target)*(x-x_target)+(y-y_target)*(y-y_target));
	if (dist>0.1) {  // don't do the first rotate and translate if goal is too close
		heading = atan2(y_target-y,x_target-x);
		diff = anglelimit(heading-theta);
		if (diff<(-0.5*PI)) {
			dist = -dist;
			diff = diff+PI;
		}
		else if (diff>(0.5*PI)) {
			dist = -dist;
			diff = diff-PI;
		}
		speed = e_rotate(diff,2.0);     		// rotate at 2 radians/sec
		while((e_get_goal_active_left()!=0) || (e_get_goal_active_right()!=0));
		theta = theta + diff;
		speed = e_translate(dist,4.0);  		// translate at 4 cm/sec (max around 12.9 cm/sec)
		while((e_get_goal_active_left()!=0) || (e_get_goal_active_right()!=0));
	}
	diff = anglelimit(theta_target - theta);
	if (abs(diff)>0.02) {             			// don't do final rotate if already close to desired angle
		speed = e_rotate(diff,2.0);     		// rotate at 2 radians/sec
		while((e_get_goal_active_left()!=0) || (e_get_goal_active_right()!=0));
	}
}

void read_incoming(void){
	int flag=0;
	while (e_getchar_uart1(&c)==0 || flag == 0){     	// function in uart/e_uart_char.h
		if(c>0){
			flag = 1;
		}
	}
	buffer[0]=c;
	i = 1;
	do if (e_getchar_uart1(&c))   				// "do" put chars in string while chars available
		buffer[i++]=c;
	while (c!='z');   					// end "do" when char is newline or return
	buffer[i++]='\0';					// end the string
}

void wait4confirm(void){
	c = 'z';
	while (c!='A'){
		e_getchar_uart1(&c);
	}
	c = 'z';
}

void send_reading(void){
	e_get_configuration(&x,&y,&theta);
	sprintf(buffer,"%fz%fz%dz",x,y,e_get_acc(0));
	uart_send_text(buffer);
}

int main(void){
	e_init_port();						// configure port pins
	e_start_agendas_processing();				// start the motor interrupt service routines
	e_init_motors();
	e_init_uart1();
	e_init_ad_scan(ALL_ADC);
	e_set_agenda_cycle(send_reading, 200);			// setup send_reading agenda in interrupt

	uart_send_static_text("Az");				// send confirmation for communication link

	wait4confirm();
	e_acc_calibr();						// calibrate sensor for black

	wait4confirm();
	sprintf(buffer,"%dz",e_get_acc(0));
	uart_send_text(buffer);					// calibrate sensor for white

	wait4confirm();
	e_set_configuration(0.0,0.0,0.0);			// calibrate e-puck's location/heading to (0,0,0)

	e_activate_agenda(send_reading, 200);			// start send_reading agenda in interrupt
	e_pause_agenda(send_reading);				// pause send_reading agenda in interrupt

	j=0;
	for(i=0;i<25;i++){
		e_restart_agenda(send_reading);			// start send_reading agenda in interrupt
		go_to(50.0,j+0.0,0.0);
		e_pause_agenda(send_reading);			// pause send_reading agenda in interrupt
		uart_send_static_text("Az");			// send confirmation for image update
		go_to(50.0,j+1.0,0.0);
		e_restart_agenda(send_reading);
		go_to(0.0,j+1.0,0.0);
		e_pause_agenda(send_reading);
		uart_send_static_text("Az");			// send confirmation for image update
		go_to(0.0,j+2.0,0.0);

		e_get_configuration(&x,&y,&theta);		// send pos/heading info
		sprintf(buffer,"%fz%fz%fz",x,y,theta*RAD2DEG);
		uart_send_text(buffer);	
	
		read_incoming();				// wait for pos/heading update
	
		sscanf(buffer,"(%f,%f,%f)z",&x,&y,&theta);	// update pos/heading info
		e_set_configuration(x,y,theta*DEG2RAD);
		j=j+2.0;
	}
	return 1;
}

Code for MATLAB

RedScan.m

The main script that starts the connection and interface.

%  RedScan.m  James Yeung 6/1/2008
%  This program is designed to communicate to an e-puck via blutooth using RS232 protocol.
%  red_scan.hex will have to be on the e-puck for this to work properly.
%  
%  The main loop of this program waits for a character input from the user,
%  upon which it transmits the ascii value and waits for data to be written.

output_file = fopen('output.txt','w');

image = zeros(500, 500, 3);

s = serial('COM6','BAUD',19200);            % Create serial object (PORT Dependent)
fopen(s);                                    % Open the serial port for r/w

inNum(64) = 0;
outChar = 'a';
ii = 0;

input('Restart e-puck and press enter.');

ReadNum;
if(inNum(1) == 65)
    fprintf('Communication link ready.\n');
else
    fprintf('Error: Restart e-puck and MATLAB program.\n');
end

input('Place e-puck in dark spot for calibration. Press enter when ready.');
fprintf(s,'%c','A');
input('Place e-puck in bright spot for calibration. Press enter when ready.');
fprintf(s,'%c','A');
ReadNum;
max_val = temp;
fprintf('Max value: %d',temp);
fprintf('\n');
input('Place e-puck in location/heading (0,0,0). Press enter when ready.');
imshow(image);
fprintf(s,'%c','A');

for iii = 1:25
    for jjj = 1:2
        ReadNum;
        while(inNum(1) ~= 65)
            x = round(temp*10);
            ReadNum;
            y = round(temp*10);
            ReadNum;
            image(500-y+1:500-y+10,x+1:x+5,1) = temp/max_val;
            ReadNum;
        end
        inNum(1) = 66;
        imshow(image);
    end
    UpdatePos;
end

outChar = input('Type "A" to get value, "Q" to quit: ','s');
while(outChar ~= 'Q')
    fprintf(s,'%c',outChar);
    ReadNum;
    fprintf('Current value: %d\n',temp);
    outChar = input('Type "A" to get value, "Q" to quit: ','s');
end


% myChar = 'a';                               
% prompt = 'Enter a character (q to exit): '; 
% 
% while (myChar ~= 'q')                       % While user hasn't typed 'q'
%     fprintf(s, '%s', myChar(1))             % Write first char of user input to serial port
%     fprintf(fscanf(s))                      % Read Data back from PIC
%     myChar = input(prompt, 's');            % Get user input
% end

fclose(output_file);
fprintf('Closing communication link...\n');
fclose(s);                                  % Close the serial port
fprintf('Communication link closed.\n');
fprintf('Deleting communication link...\n');
delete(s);                                  % Delete the serial object
fprintf('Communication link deleted.\n');
fprintf('Program terminated.\n');

ReadNum.m

A sub routine to read from incoming RS232 stream. Stores individual incoming characters in "inNum(ii)" and the final number in "temp".

ii = 1;
inNum(ii) = fread(s,1);
while(inNum(ii) ~= 122)
    ii = ii + 1;
    inNum(ii) = fread(s,1);
end

ii = 1;
while(inNum(ii) ~= 0)
    buffer(ii) = cast(inNum(ii),'char');
    ii = ii + 1;
end

temp = sscanf(buffer,'%f',1);

UpdatePos.m

A sub routine to ask user to manually input position/heading coordinates to update the e-Puck.

fprintf('e-puck thinks its position/heading is (');
ReadNum;
fprintf('%f,',temp);
ReadNum;
fprintf('%f,',temp);
ReadNum;
fprintf('%f)\n',temp);
outChar = input('Enter real position/heading (x,y,theta): ','s');
fprintf(s,'%c',outChar);
fprintf(s,'%c','z');

ErrorPlot.m

A routine that is called at the end of the program to plot the error between captured image and theoretical image.

real_image = zeros(500,500,3);
for x = 1:500
    for y = 1:500
        real_image(y,x,1) = 1 - sqrt(abs(250-x)^2+abs(250-y)^2)/300;
    end
end

error_image = zeros(500,500);
for x = 1:500
    for y = 1:500
        error_image(x,y) = image(y,x,1) - real_image(y,x,1);
    end
end

[x,y]=meshgrid(1:500,1:500);
surf(x,y,error_image,'EdgeColor','none')

Results

The 3D error plots give you the percentage error for each of the points of the captured image compared to the projected image. Ideally, the graph would be flat with the z-intercept at 0. A negative z value indicates a lower than expected value for that coordinate.

The first trial run covered the entire image. The spikes are a glitch that happens when collecting readings. I'm not exactly sure why there are spikes. I also forgot to capture the reproduced image of the first full trial run. In this run, I simply allowed the e-Puck to perform the full lawn mowing pattern to cover the entire image. Initially, I tried to simply feed the e-puck with updated coordinates, but it didn't seem to work very well because each time it would get more and more off course. So I decided to manually move the e-Puck to the correct each time it asked me to update its position coordinates.

Side view of the error plot. This view demonstrates the overall accuracy of the captured image. The thinner the better. Here, besides the spikes, the error seems to be within 30%.

Another view of the plot.

Top view of the plot.

The second trial run demonstrates the e-Puck's ability to adjust to manual movement of the e-Puck. When the e-Puck prompts for an update for its position coordinates, I would move the e-Puck to a dramatically different position and give it the coordinates at which I put it at. As you can see from the plots, the diagonal lines are the new path that the e-Puck chose to take due to that movement.

The reconstructed image. Color was distorted by MATLAB when saving to bmp. During the experiment, the shades of red were much smoother.

Top diagonal view of the plot. Because I did not cover the entire image in this run, the majority of the plot is simply the dummy data "0".

From the side, we can see that for the vertical path that the e-Puck took, the error were within 20%. For the diagonal paths, we can't see from this angle.

A top view of the plot for the second run.

Possible Future Improvements

• To allow the user to manually input a path for the e-Puck to explore.