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); } }