Pololu 3pi Robot - 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 © 2001–2014 Pololu Corporation
10. Expansion Information Page 51 of 63