Pololu 3pi Robot User Manual

Page 51

serial_set_mode(SERIAL_AUTOMATIC);

unsigned int position = read_line(sensors, IR_EMITTERS_ON);

serial_set_mode(SERIAL_CHECK);

// The "proportional" term should be 0 when we are on the line.

int proportional = ((int)position) - 2000;

// Compute the derivative (change) of the position.

int derivative = proportional - last_proportional;

// Remember the last position.

last_proportional = proportional;

// Compute the difference between the two motor power settings,

// m1 - m2. If this is a positive number the robot will turn

// to the right. If it is a negative number, the robot will

// turn to the left, and the magnitude of the number determines

// the sharpness of the turn.

int power_difference = proportional*p_num/p_den + derivative*p_num/p_den;

// Compute the actual motor settings. We never set either motor

// to a negative value.

if(power_difference > max_speed)

power_difference = max_speed;

if(power_difference < -max_speed)

power_difference = -max_speed;

if(power_difference < 0)

set_motors(max_speed+power_difference, max_speed);

else

set_motors(max_speed, max_speed-power_difference);

}

// A global ring buffer for data coming in. This is used by the

// read_next_byte() and previous_byte() functions, below.

char buffer[100];

// A pointer to where we are reading from.

unsigned char read_index = 0;

// Waits for the next byte and returns it. Runs play_check to keep

// the music playing and serial_check to keep receiving bytes.

// Calls pid_check() to keep following the line.

char read_next_byte()

{

while(serial_get_received_bytes() == read_index)

{

serial_check();

play_check();

// pid_check takes some time; only run it if we don't

// have more bytes to process

if(serial_get_received_bytes() == read_index)

pid_check();

}

char ret = buffer[read_index];

read_index ++;

if(read_index >= 100)

read_index = 0;

return ret;

}

// Backs up by one byte in the ring buffer.

void previous_byte()

{

read_index --;

if(read_index == 255)

read_index = 99;

}

// Returns true if and only if the byte is a command byte (>= 0x80).

char is_command(char byte)

{

if (byte < 0)

return 1;

return 0;

Pololu 3pi Robot User's Guide

10. Expansion Information

Page 51 of 63