E-Puck Color Sensing Project

The 'Steel Toe' programmable stiffness joint

Overview

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.

image1 image2 image3 image4

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.

image1 image2 image3 image4

Results and Reflections

Overall Results

Overall, our project was a great success. There were things that could have been done differently and can be improved, but we managed to achieve what we have set out to do, which was to make a device that models a joint and have the ability to change its rotational stiffness. We hope that "Steel Toe" will aid researchers in creating better prosthetic legs and joints for people in need.

In terms of obtaining some quantitative results, by testing to see how much horizontal force was needed to pull the shank to 90 degrees, we found that "Steel Toe" was able to achieve a factor of about 5 in the range of stiffnesses. Here's a table of our test results.

Stiffness Level	Force Needed to Get to 90 Degrees
0	2.943 N
1	3.434 N
2	4.415 N
3	5.886 N
4	7.358 N
5	9.810 N
6	11.282 N
7	12.263 N
8	13.734 N
9	14.715 N

Possible Mechanical Improvements

• Move circuitry to shank.
• Reduce weight of foot plate.
• Use faster motor for spinning the rod (or gear up current motor) to increase adjustment speed.

Possible Electrical/Software Improvements

• Have the program continuously check for changes in dial. Currently it is only checking when motors are not moving.
• Use a different H-bridge chip to replace existing diodes. L293D instead of L293B.

Future Directions

The actual stiffness of the joint is not constant over the range of motion. For this limited range of motion, a linear approximation is valid, however more precise computations accounting for the non-linearity could be of use.

This device could be modified in design to incorporate more powerful motors to drive the slider faster and even possibly during loading to achieve a stiffness which can change during the gait cycle to adapt to conditions or simply better conform to natural gait kinetics.

Following this device with a model incorporating programmable damping would be another step toward differentiating between the elastic stiffness and damping properties of the ankle joint during walking.