예제 #1
0
/** readSensorsBranches ******************************************
 * Read the IR line sensors and return an interpreted result.
 *
 * Sensors are numbered 0 - 4 starting at the left. This version
 * returns a value as for readLineSensors(), but also sets isLeft
 * and isRight to indicate if there is a line under sensor 0 (isLeft) and/or
 * sensor 4 (isRight). If either isLeft or isRight is true then the value returned
 * shouldn't be trusted for steering since
 *
 * @modifies isLeft is true if and only if there is a line detected directly
 *               under the left sensor (0)
 *           isRight is true if and only if there is a line detected directly
 *               under the right sensor (4)
 * @returns if !(isLeft || isRight) then the value can be interpreted as follows:
 *          0           -- line sensed under leftmost sensor (0) only
 *                         or no line detected
 *          1 - 999     -- line sensed under left two sensors (0 and 1)
 *          1000 - 1999 -- line sensed under sensors 1 and 2
 *          2000        -- line sensed under sensors 1, 2 and 3
 *          2001 - 2999 -- line sensed under sensors 2 and 3
 *          3000 - 3999 -- line sensed under sensors 3 and 4
 *          4000        -- line sensed under rightmost sensor (4) only
 *                         or no line detected
 */
int readSensorsBranches(bool& isLeft, bool& isRight) {
	unsigned int sensors[5];
	int result = read_line(sensors, IR_EMITTERS_ON);
	isLeft = sensors[0] > 500;
	isRight = sensors[4] > 500;
	display_readings(sensors);
	return result;
}
예제 #2
0
/**lineCalibration*****************************************
* @descrip: a function to calibrate the sensors and display
* the results in the form of a bar graph. It waits for a button
* to be pressed.
* @param: none
* @returns: BUTTON_A, BUTTON_B or BUTTON_C depending on which was
*           pressed after the calibration
*
***********************************************************/
unsigned char lineCalibration() {

	const int TURN_SPEED = 40; // motor speed for turning
	const int TURN_STEPS = 20; // number of samples in a half turn
	const int SAMPLE_DELAY = 20; // ms between samples
	unsigned char button; // which button is pressed at the end

	set_motors(-TURN_SPEED, TURN_SPEED); // start turning left
	for (int i = 0; i < TURN_STEPS; i++) {// some number of times
		calibrate_line_sensors(IR_EMITTERS_ON);
		delay_ms(SAMPLE_DELAY);           // 		wait 20ms
	}
	set_motors(TURN_SPEED, -TURN_SPEED); // start turning right
	for (int i = 0; i < 2*TURN_STEPS; i++) {// twice as many times
		calibrate_line_sensors(IR_EMITTERS_ON);
		delay_ms(SAMPLE_DELAY);           // 		wait 20ms
	}
	set_motors(-TURN_SPEED, TURN_SPEED); // start turning left
	for (int i = 0; i < TURN_STEPS; i++) {// some number of times
		calibrate_line_sensors(IR_EMITTERS_ON);
		delay_ms(SAMPLE_DELAY);           // 		wait 20ms
	}
	set_motors(0,0);

	// Display calibrated values as a bar graph.
	unsigned int sensor[5]; // place to store sensor readings
	while ((button = button_is_pressed(ANY_BUTTON)) == 0) {// as long as a button is not pressed
		unsigned int position = read_line(sensor, IR_EMITTERS_ON); //    read the sensors
		clear();
		print_long(position); //    display the value on the top line
		display_readings(sensor); 	//    display the bar graph on the bottom line
		delay_ms(100);             //    wait a bit
	}
	play_from_program_space(beep_button_b); // beep for B button
	while (button_is_pressed(button)) ;   // empty loop - wait until button released
	delay_ms(200); // wait a bit more
	return button;
}
예제 #3
0
파일: test.c 프로젝트: Chen-Zhe/MDP-Grp2
// Initializes the 3pi, displays a welcome message, calibrates, and
// plays the initial music.
void initialize()
{
	unsigned int counter; // used as a simple timer
	unsigned int sensors[5]; // an array to hold sensor values

	// This must be called at the beginning of 3pi code, to set up the
	// sensors.  We use a value of 2000 for the timeout, which
	// corresponds to 2000*0.4 us = 0.8 ms on our 20 MHz processor.
	pololu_3pi_init(2000);
	load_custom_characters(); // load the custom characters
	
	// Play welcome music and display a message
	print_from_program_space(welcome_line1);
	lcd_goto_xy(0,1);
	print_from_program_space(welcome_line2);
	play_from_program_space(welcome);
	delay_ms(1000);

	clear();
	print_from_program_space(demo_name_line1);
	lcd_goto_xy(0,1);
	print_from_program_space(demo_name_line2);
	delay_ms(1000);

	// Display battery voltage and wait for button press
	while(!button_is_pressed(BUTTON_B))
	{
		int bat = read_battery_millivolts();

		clear();
		print_long(bat);
		print("mV");
		lcd_goto_xy(0,1);
		print("Press B");

		delay_ms(100);
	}

	// Always wait for the button to be released so that 3pi doesn't
	// start moving until your hand is away from it.
	wait_for_button_release(BUTTON_B);
	delay_ms(1000);

	// Auto-calibration: turn right and left while calibrating the
	// sensors.
	for(counter=0;counter<80;counter++)
	{
		if(counter < 20 || counter >= 60)
			set_motors(40,-40);
		else
			set_motors(-40,40);

		// This function records a set of sensor readings and keeps
		// track of the minimum and maximum values encountered.  The
		// IR_EMITTERS_ON argument means that the IR LEDs will be
		// turned on during the reading, which is usually what you
		// want.
		calibrate_line_sensors(IR_EMITTERS_ON);

		// Since our counter runs to 80, the total delay will be
		// 80*20 = 1600 ms.
		delay_ms(20);
	}
	set_motors(0,0);

	// Display calibrated values as a bar graph.
	while(!button_is_pressed(BUTTON_B))
	{
		// Read the sensor values and get the position measurement.
		unsigned int position = read_line(sensors,IR_EMITTERS_ON);

		// Display the position measurement, which will go from 0
		// (when the leftmost sensor is over the line) to 4000 (when
		// the rightmost sensor is over the line) on the 3pi, along
		// with a bar graph of the sensor readings.  This allows you
		// to make sure the robot is ready to go.
		clear();
		print_long(position);
		lcd_goto_xy(0,1);
		display_readings(sensors);

		delay_ms(100);
	}
	wait_for_button_release(BUTTON_B);

	clear();

	print("Go!");		

	// Play music and wait for it to finish before we start driving.
	play_from_program_space(go);
	while(is_playing());
}
예제 #4
0
/** readLineSensors ******************************************
 * Read the IR line sensors and return an interpreted result.
 *
 * Sensors are numbered 0 - 4 starting at the left. The interpretation
 * assumes that the robot is following a 3/4" wide black line with no
 * sharp turns and no branches.
 *
 * @returns 0           -- line sensed under leftmost sensor (0) only
 *                         or no line detected
 *          1 - 999     -- line sensed under left two sensors (0 and 1)
 *          1000 - 1999 -- line sensed under sensors 1 and 2
 *          2000        -- line sensed under sensors 1, 2 and 3
 *          2001 - 2999 -- line sensed under sensors 2 and 3
 *          3000 - 3999 -- line sensed under sensors 3 and 4
 *          4000        -- line sensed under rightmost sensor (4) only
 *                         or no line detected
 */
int readLineSensors() {
	unsigned int sensors[5];
	int result = read_line(sensors, IR_EMITTERS_ON);
	display_readings(sensors);
	return result;
}