void ir_test()
{
unsigned int sensors[5]; // an array to hold sensor values
if(button_is_pressed(BUTTON_C))
read_line_sensors(sensors, IR_EMITTERS_OFF);
else
read_line_sensors(sensors,IR_EMITTERS_ON);
unsigned char i;
for(i=0; i<5; i++) {
// Initialize the array of characters that we will use for the
// graph. Using the space, an extra copy of the one-bar
// character, and character 255 (a full black box), we get 10
// characters in the array.
// The variable c will have values from 0 to 9, since
// values are in the range of 0 to 2000, and 2000/201 is 9
// with integer math.
char c = bar_graph_characters[sensors[i]/201];
// Display the bar graph characters.
print_character(c);
}
// Display an indicator of whether IR is on or off
if(button_is_pressed(BUTTON_C))
print("IR-");
else
print(" C");
delay_ms(100);
}
//Calibrates the sensor
void calibrate(unsigned int *sensors, unsigned int *minv, unsigned int *maxv) {
//say something to the user
clear();
lcd_goto_xy(0, 0);
print(" Fluffy");
lcd_goto_xy(0, 1);
print("A=Go!");
//wait on the calibration button
wait_for_button_press(BUTTON_A);
//wait for the user to move his hand
delay_ms(500);
//activate the motors
set_motors(40, -40);
//take 165 readings from the sensors...why not?
int i;
for (i = 0; i < 165; i++) {
read_line_sensors(sensors, IR_EMITTERS_ON);
update_bounds(sensors, minv, maxv);
delay_ms(10);
}
//and turn the motors off, we're done
set_motors(0, 0);
delay_ms(750);
}
// Reads the line sensors and sends their values. This function can
// do either calibrated or uncalibrated readings. When doing calibrated readings,
// it only performs a new reading if we are not in PID mode. Otherwise, it sends
// the most recent result immediately.
void send_sensor_values(char calibrated)
{
if(calibrated)
{
if(!pid_enabled)
read_line_sensors_calibrated(sensors, IR_EMITTERS_ON);
}
else
read_line_sensors(sensors, IR_EMITTERS_ON);
serial_send_blocking((char *)sensors, 10);
}
void sensor_graph()
{
unsigned int values[5];
clear();
while(!button_is_pressed(BUTTON_B))
{
read_line_sensors(values,IR_EMITTERS_ON);
display_values(values,TEST_LINE_SENSOR_TIMEOUT);
lcd_goto_xy(6,1);
print("B");
delay_ms(50);
}
while(button_is_pressed(ALL_BUTTONS));
}
void check_emitter_jumper()
{
unsigned int values[5];
clear();
// off values
while(!button_is_pressed(BUTTON_C))
{
read_line_sensors(values,IR_EMITTERS_OFF);
lcd_goto_xy(0,0);
print("IR- ");
display_values(values,TEST_LINE_SENSOR_TIMEOUT);
lcd_goto_xy(6,1);
print("C");
delay_ms(50);
}
while(button_is_pressed(ALL_BUTTONS));
}
void test_qtr()
{
unsigned int values[5];
clear();
// Wait for each sensor to be > 750 while the others are < 250.
unsigned int passed_sensors[5] = {0,0,0,0,0};
while(!button_is_pressed(BUTTON_B))
{
read_line_sensors(values,IR_EMITTERS_ON);
unsigned char i;
unsigned char sensor_above=0;
char num_above=0;
char num_below=0;
for(i=0;i<5;i++)
{
if(values[i] > 750)
{
sensor_above = i;
num_above ++;
}
else if(values[i] < 500)
num_below ++;
}
if(num_above == 1 && num_below == 4)
passed_sensors[sensor_above] = 1;
lcd_goto_xy(0,0);
for(i=0;i<5;i++)
{
if(passed_sensors[i])
print_character('*');
else
print_character(' ');
}
display_values(values,1000);
lcd_goto_xy(6,1);
print("B");
delay_ms(50);
}
while(button_is_pressed(ALL_BUTTONS));
clear();
// off values
while(!button_is_pressed(BUTTON_C))
{
read_line_sensors(values,IR_EMITTERS_OFF);
lcd_goto_xy(0,0);
print("IR- ");
display_values(values,1000);
lcd_goto_xy(6,1);
print("C");
delay_ms(50);
}
while(button_is_pressed(ALL_BUTTONS));
}
//This is the main function, where the code starts. All C programs
//must have a main() function defined somewhere.
int main() {
//holds sensor values
unsigned int sensors[5];
//hold min and max sensor values for calibration
unsigned int minv[5] = {65500, 65500, 65500, 65500, 65500}, maxv[5] = {0, 0, 0, 0, 0};
//holds the previous value so that we can make sure the sensor didn't report a crap value
int range[5];
//set up the 3pi
pololu_3pi_init(2000);
//calibrate the stuff
calibrate(sensors, minv, maxv);
//set our range from our calibrated readings so that it doesn't break the other readings when we start moving
int i;
for (i = 0; i < 5; i++)
range[i] = getCalibratedSensor(sensors[i], minv[i], maxv[i]);
//set the speed
int const speed = 255;
//holds the deriv
int deriv;
//holds the integral
int integ = 0;
//holds the last position
int lastProp = 0;
//line position relative to center
int position = 0;
long last = millis();
//run in circles
while(1) {
//read the line sensor values
read_line_sensors(sensors, IR_EMITTERS_ON);
//compute line positon
position = line_position(sensors, minv, maxv, range);
//get the middle sensors to = 0
int prop = position - 250;
//save the running time
long now = millis();
long diff = now - last;
//calc the derivative
deriv = ((prop - lastProp) * 10) / diff;
//if the robot has changed directions, clear the integral
if ((lastProp < 0 && prop > 0) || (prop < 0 && lastProp > 0))
integ = 0;
else
integ += prop * diff;
//get a proportional speed
int propSpeed = (prop * 2) + (integ / 6500) + (deriv * 23);
//set our last run time
last = now;
//get a proportional speed
int left = speed+propSpeed;
int right = speed-propSpeed;
//make sure the motors are never off / going negative
if (left <= 0) {
int diff = 0 - left;
right += diff - 30;
left = 30;
}
if (right <= 0) {
int diff = 0 - right;
left += diff - 30;
right = 30;
}
//limit the motors to their maxes
if (left > 255)
left = 255;
if (right > 255)
right = 255;
lastProp = prop;
set_motors(left, right);
}
}
