Leader Follower Coding

Light is an important factor when dealing with vision. While testing tracking different objects lighting became an obstacle. The NXTCam is to be used under florescent light, any other lights might distract the NXTCam which leads to it losing the object. Testing proved that using a light source on the leader robot in a dark environment provides the camera with an object that is relatively hard to lose. Also, it increases the range of the NXT camera to allow the follower robot to find the leader from farther distances.

Positioning the light on the NXT was a bit challenging. The light has to be right above the straight line that comes out of the NXTCam center. It can be at the same level because it affects the cameras ability to track the object effectively, it’s like someone to shine a light straight in your eyes. So I had to build a frame to hold the light source. Building the frame was a bit hard due to the fact that I had to do it with Lego pieces and it was hard to find the perfect spot for the light and the angle so it won’t disturb the NXTCam.

The positioning of the camera was the bottom of the NXT looking straight ahead. I changed the position of the NXTCam to be on top looking down. The new position was better than the old one. Testing showed that there are less chances of the NXT losing the object when the camera was put in the new position. On the other hand, the old position had better range.

Time: 6 hrs.

More NXTCam

Lighting conditions

The NXTCam is designed to operate under white fluorescent light. If you notice

reddish image color, which tends to happen when your environment has lot of

ambient Infrared light, try to find the source of Infrared light and reduce it by

replacing it with fluorescent light.

For advanced operations it is possible to change color gain, brightness and contrast

of NXTCam by manipulating the I2C register values.

Following table lists the register definitions and setup commands:

Commands

ASCII Hex Action

A 0x41 Sort tracked objects by size

B 0x42 Select Object tracking mode

C 0x43 Write to Camera Registers

D 0x 44 Disable Tracking

E 0x45 Enable Tracking

G 0x47 Get the Color map from Camera Engine

H 0x48 Read data from the Camera Registers

I 0x49 Illumination on (Future)

L 0x 4C Select Line tracking mode

N 0x4E Set ADPA mode ON (setting stored in NVRAM)

O 0x4F Set ADPA mode Off (default) (setting stored in NVRAM)

P 0x50 Ping camera Engine

R 0x52 Reset Camera Engine

S 0x53 Send the color map to camera Engine

T 0x54 Illumination Off

U 0x55 Sort tracked objects by color

V 0x56 Get camera Engine firmware version No

X 0x58 Do not Sort tracked objects

Time: 5hrs.

A different way to use NXTCam.

A different way to use NXTCam.

This code below is a new way of using the NXT cam. Its different because it allows more control over the NXTCam, but after experimenting it, it didn’t show enough potential for me to research more about it, nor use it in my programming.


#define I2C_PORT S1
#define MOVE_TIME 300
#define TURN_TIME 75
#define BIG_TURN_TIME 250

int err;
byte send[] = {0x40, 0xfe};
byte recv[] = {1};

sub turn_right()
{
OnFwd(OUT_A, 60);
OnRev(OUT_C,55);
Wait(TURN_TIME);
Off(OUT_AC);
}

sub sendrecvi2c (byte data)
{
ClearScreen ();
send[0] = 0x40;
send[1] = data;
while (I2CCheckStatus(I2C_PORT) != 0);
err = I2CWrite(I2C_PORT, 1, send);
while (I2CCheckStatus(I2C_PORT) != 0);
TextOut (0, LCD_LINE4,"Clear");
while (I2CBytesReady(I2C_PORT) < 1);
I2CRead(I2C_PORT, 1, recv);
TextOut (0, LCD_LINE5,"Read");
NumOut(60, LCD_LINE5, recv[0]);
Wait(500);
}

sub turn_left()
{
OnFwd(OUT_C, 60);
OnRev(OUT_A,55);
Wait(TURN_TIME);
Off(OUT_AC);
}

sub turn_big_right()
{
OnFwd(OUT_A, 60);
OnRev(OUT_C,55);
Wait(BIG_TURN_TIME);
Off(OUT_AC);
}

sub turn_big_left()
{
OnFwd(OUT_C, 60);
OnRev(OUT_A,55);
Wait(BIG_TURN_TIME);
Off(OUT_AC);
}

sub straight()
{
OnFwd(OUT_AC, 60);
Wait(MOVE_TIME);
Off(OUT_AC);
}


task main()
{
SetSensorType(I2C_PORT, SENSOR_TYPE_LOWSPEED);
SetSensorMode(I2C_PORT, IN_MODE_RAW);
ResetSensor(I2C_PORT);
while(true)
{
sendrecvi2c(0x62);
Wait(100);
sendrecvi2c(0x62);
if(recv[0]>247)
{
TextOut (0, LCD_LINE2,"Error");
turn_big_right();
}
else if(recv[0]>90)
{
TextOut (0, LCD_LINE2,"Turn Right");
turn_right();
}
else if(recv[0]<75)
{
TextOut (0, LCD_LINE2,"Turn Left");
turn_left();
}
else
{
TextOut (0, LCD_LINE2,"Ball found and centered");
straight();
}
Wait(100);
}
ClearScreen ();
TextOut (0, LCD_LINE2,"Done");
Wait(10000);
}

Time: 4 hrs

Movement

I was interested in using a particle filter to provide the robots more stable movement. The particle filter uses three ultrasonic sensors to provide the NXT with an idea about its location in the surrounding environment. The particle filter is based on a style of probabilistic localization called Monte-Carlo Localization. It relies on finding the best odds from a series of seemingly random outcomes. Particle filters are horribly computationally intensive with lots of floating point math.

In Monte-Carlo Localization a large number of hypothetical current configurations are initially randomly scattered in configuration space. With each sensor update, the probability that each hypothetical configuration is correct is updated based on a statistical model of the sensors and Bayes' theorem. Similarly, every motion the robot undergoes is applied in a statistical sense to the hypothetical configurations based on a statistical motion model. When the probability of a hypothetical configuration becomes very low, it is replaced with a new random configuration.

Due to the time limit, I didn’t have a chance to work much on coding it and using with my algorithm, so I will not be working on this anymore, but I will still do personal research about this subject at a later time.

Time: 6hrs

Using light sensors

I was interested in using light sensors to achieve the Leader-Follower algorithm. I used two light sensors on each side of the NXT to detect an LED light that is mounted on the NXT. I started with a simple algorithm, coded it and tested it. It worked okay but modifications were needed so I changed the code to send more specific directions to the NXT. More modifications were made to achieve better results. At the end, this algorithm didn’t show enough potential to compete with the NXTCam. The code below shows the three different stages I went through coding and improving my algorithm.

1. task main (){

while (true) {

wait1Msec(50); //the program waits 50 millisecond to initialize the light sensor

while((SensorValue(lightSensor1) - SensorValue(lightSensor2)) {

motor[motorB] = 40;

motor[motorC] = 40;

}

while(SensorValue(lightSensor1) < style=""> {

motor[motorB] = 40; //motor A is run at a 100 power level

motor[motorC] = -40; //motor B is run at a 100 power level

}

while(SensorValue(lightSensor1) > SensorValue(lightSensor2) ) {

motor[motorB] = -40; //motor A is run at a 100 power level

motor[motorC] = 40; //motor B is run at a 100 power level

} } }

2.

task main (){

while (true) {

wait1Msec(50); //the program waits 50 millisecond to initialize the light sensor

int right = SensorValue(lightSensor1);

int left = SensorValue(lightSensor2);

motor[motorB] = (left - right) * 5 ;

motor[motorC] = (right - left) * 5 ;

} }

3.

task main (){

while (true) {

wait1Msec(50); //the program waits 50 millisecond to initialize the light sensor

int right = SensorValue(lightSensor1);

int left = SensorValue(lightSensor2);

motor[motorB] = left * 5 ;

motor[motorC] = right * 5 ;

} }

Time: 7 hrs.

Light and Sonar Sensors in RobotC

This code below allows the NXT to run forward until it detects a light surface:

const tSensors lightSensor = (tSensors) S1; //sensorLightActive

task main()

{

wait1Msec(50); //the program waits 50 millisecond to initialize the light sensor

while(SensorValue(lightSensor) //a loop that will keep looping while the light sensor's value is less than 45.

{

motor[motorA] = 100; //motor A is run at a 100 power level

motor[motorB] = 100; //motor B is run at a 100 power level

}

motor[motorA] = 0; //motor A is stopped with a 0 power level

motor[motorB] = 0; //motor B is stopped with a 0 power level

}

This code below allows the NXT to move forward until it detects an obstacle and then stops.

const tSensors sonarSensor = (tSensors) S1; //sensorSONAR

task main()

{

while(SensorValue[sonarSensor] > 20)

{

motor[motorA] = 75; //motor A is run at a 75 power level

motor[motorB] = 75; //motor B is run at a 75 power level

}

}

At this stage I am very familiar with most of the function that are needed and I understand all abilities of RobotC that is needed for my research.

Time: 4 hrs.