Differences

This shows you the differences between two versions of the page.

--- et:projects:3pi:solutions [2015/11/12 12:22] – kaupo.raid
+++ et:projects:3pi:solutions [2020/07/20 09:00] (current) – external edit 127.0.0.1
@@ Line 147: / Line 147: @@
 ==3Pi PID regulaatoriga joonejärgimine==
 <code c>
 /*
  * 3pi-linefollower-pid - demo code for the Pololu 3pi Robot
  *
  * This code will follow a black line on a white background, using a
  * PID-based algorithm.
@@ Line 158: / Line 159: @@
  *
  */
 // The 3pi include file must be at the beginning of any program that
 // uses the Pololu AVR library and 3pi.
 #include <pololu/3pi.h>
-//#include <avr/eeprom.h>
 // This include file allows data to be stored in program space.  The
 // ATmega168 has 16k of program space compared to 1k of RAM, so large
 // pieces of static data should be stored in program space.
 #include <avr/pgmspace.h>
-uint8_t pval;
-uint8_t ival;
-uint8_t dval1;
-uint8_t dval2;
 // Introductory messages.  The "PROGMEM" identifier causes the data to
 // go into program space.
@@ Line 178: / Line 175: @@
 const char demo_name_line1[] PROGMEM = "PID Line";
 const char demo_name_line2[] PROGMEM = "follower";
 // A couple of simple tunes, stored in program space.
 const char welcome[] PROGMEM = ">g32>>c32";
 const char go[] PROGMEM = "L16 cdegreg4";
 // Data for generating the characters used in load_custom_characters
 // and display_readings.  By reading levels[] starting at various
 // offsets, we can generate all of the 7 extra characters needed for a
 // bargraph.  This is also stored in program space.
-const char levels[] PROGMEM =
+const char levels[] PROGMEM = {
-{
+b00000,
 b00000,
 b00000,
 b00000,
 b00000,
 b00000,
 b00000,
-b00000,
+b11111,
 b11111,
 b11111,
 b11111,
 b11111,
 b11111,
-b11111,
+b11111
-b11111
 };
 // This function loads custom characters into the LCD.  Up to 8
 // characters can be loaded; we use them for 7 levels of a bar graph.
 void load_custom_characters()
 {
-    lcd_load_custom_character(levels+0,0); // no offset, e.g. one bar
+	lcd_load_custom_character(levels+0,0); // no offset, e.g. one bar
-    lcd_load_custom_character(levels+1,1); // two bars
+	lcd_load_custom_character(levels+1,1); // two bars
-    lcd_load_custom_character(levels+2,2); // etc...
+	lcd_load_custom_character(levels+2,2); // etc...
-    lcd_load_custom_character(levels+3,3);
+	lcd_load_custom_character(levels+3,3);
-    lcd_load_custom_character(levels+4,4);
+	lcd_load_custom_character(levels+4,4);
-    lcd_load_custom_character(levels+5,5);
+	lcd_load_custom_character(levels+5,5);
-    lcd_load_custom_character(levels+6,6);
+	lcd_load_custom_character(levels+6,6);
-    clear(); // the LCD must be cleared for the characters to take effect
+	clear(); // the LCD must be cleared for the characters to take effect
 }
 // This function displays the sensor readings using a bar graph.
 void display_readings(const unsigned int *calibrated_values)
 {
-    unsigned char i;
+	unsigned char i;
-    for(i=0; i<5; i++)
+	for(i=0;i<5;i++) {
-    {
+		// Initialize the array of characters that we will use for the
-        // Initialize the array of characters that we will use for the
+		// graph.  Using the space, an extra copy of the one-bar
-        // graph.  Using the space, an extra copy of the one-bar
+		// character, and character 255 (a full black box), we get 10
-        // character, and character 255 (a full black box), we get 10
+		// characters in the array.
-        // characters in the array.
+		const char display_characters[10] = {' ',0,0,1,2,3,4,5,6,255};
-        const char display_characters[10] = {' ',0,0,1,2,3,4,5,6,255};
+		// The variable c will have values from 0 to 9, since
-        // The variable c will have values from 0 to 9, since
+		// calibrated values are in the range of 0 to 1000, and
-        // calibrated values are in the range of 0 to 1000, and
+		// 1000/101 is 9 with integer math.
-        // 1000/101 is 9 with integer math.
+		char c = display_characters[calibrated_values[i]/101];
-        char c = display_characters[calibrated_values[i]/101];
+		// Display the bar graph character.
-        // Display the bar graph character.
+		print_character(c);
-        print_character(c);
+	}
-    }
 }
 // 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 counter; // used as a simple timer
-    unsigned int sensors[5]; // an array to hold sensor values
+	unsigned int sensors[5]; // an array to hold sensor values
-    // This must be called at the beginning of 3pi code, to set up the
+	// 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
+	// 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.
+	// corresponds to 2000*0.4 us = 0.8 ms on our 20 MHz processor.
-    pololu_3pi_init(2000);
+	pololu_3pi_init(2000);
-    load_custom_characters(); // load the custom characters
+	load_custom_characters(); // load the custom characters
-    // Play welcome music and display a message
+	// Play welcome music and display a message
-    print_from_program_space(welcome_line1);
+	print_from_program_space(welcome_line1);
-    lcd_goto_xy(0,1);
+	lcd_goto_xy(0,1);
-    print_from_program_space(welcome_line2);
+	print_from_program_space(welcome_line2);
-    play_from_program_space(welcome);
+	play_from_program_space(welcome);
-    delay_ms(1000);
+	delay_ms(1000);
-    clear();
+	clear();
-    print_from_program_space(demo_name_line1);
+	print_from_program_space(demo_name_line1);
-    lcd_goto_xy(0,1);
+	lcd_goto_xy(0,1);
-    print_from_program_space(demo_name_line2);
+	print_from_program_space(demo_name_line2);
-    delay_ms(1000);
+	delay_ms(1000);
-    // Display battery voltage and wait for button press
+	// Display battery voltage and wait for button press
-    while(!button_is_pressed(BUTTON_B))
+	while(!button_is_pressed(BUTTON_B))
-    {
+	{
-        int bat = read_battery_millivolts();
+		int bat = read_battery_millivolts();
-        clear();
+		clear();
-        print_long(bat);
+		print_long(bat);
-        print("mV");
+		print("mV");
-        lcd_goto_xy(0,1);
+		lcd_goto_xy(0,1);
-        print("Press B");
+		print("Press B");
-        delay_ms(100);
+		delay_ms(100);
-    }
+	}
-    // Always wait for the button to be released so that 3pi doesn't
+	// Always wait for the button to be released so that 3pi doesn't
-    // start moving until your hand is away from it.
+	// start moving until your hand is away from it.
-    wait_for_button_release(BUTTON_B);
+	wait_for_button_release(BUTTON_B);
-    delay_ms(1000);
+	delay_ms(1000);
-    // Auto-calibration: turn right and left while calibrating the
+	// Auto-calibration: turn right and left while calibrating the
-    // sensors.
+	// sensors.
-    for(counter=0; counter<80; counter++)
+	for(counter=0;counter<80;counter++)
-    {
+	{
-        if(counter < 20 || counter >= 60)
+		if(counter < 20 || counter >= 60)
-            set_motors(40,-40);
+			set_motors(40,-40);
-        else
+		else
-            set_motors(-40,40);
+			set_motors(-40,40);
-        // This function records a set of sensor readings and keeps
+		// This function records a set of sensor readings and keeps
-        // track of the minimum and maximum values encountered.  The
+		// track of the minimum and maximum values encountered.  The
-        // IR_EMITTERS_ON argument means that the IR LEDs will be
+		// IR_EMITTERS_ON argument means that the IR LEDs will be
-        // turned on during the reading, which is usually what you
+		// turned on during the reading, which is usually what you
-        // want.
+		// want.
-        calibrate_line_sensors(IR_EMITTERS_ON);
+		calibrate_line_sensors(IR_EMITTERS_ON);
-        // Since our counter runs to 80, the total delay will be
+		// Since our counter runs to 80, the total delay will be
-        // 80*20 = 1600 ms.
+		// 80*20 = 1600 ms.
-        delay_ms(20);
+		delay_ms(20);
-    }
+	}
-    set_motors(0,0);
+	set_motors(0,0);
-    // Display calibrated values as a bar graph.
+	// Display calibrated values as a bar graph.
-    while(!button_is_pressed(BUTTON_B))
+	while(!button_is_pressed(BUTTON_B))
-    {
+	{
-        // Read the sensor values and get the position measurement.
+		// Read the sensor values and get the position measurement.
-        unsigned int position = read_line(sensors,IR_EMITTERS_ON);
+		unsigned int position = read_line(sensors,IR_EMITTERS_ON);
-        // Display the position measurement, which will go from 0
+		// Display the position measurement, which will go from 0
-        // (when the leftmost sensor is over the line) to 4000 (when
+		// (when the leftmost sensor is over the line) to 4000 (when
-        // the rightmost sensor is over the line) on the 3pi, along
+		// the rightmost sensor is over the line) on the 3pi, along
-        // with a bar graph of the sensor readings.  This allows you
+		// with a bar graph of the sensor readings.  This allows you
-        // to make sure the robot is ready to go.
+		// to make sure the robot is ready to go.
-        clear();
+		clear();
-        print_long(position);
+		print_long(position);
-        lcd_goto_xy(0,1);
+		lcd_goto_xy(0,1);
-        display_readings(sensors);
+		display_readings(sensors);
-        delay_ms(100);
+		delay_ms(100);
-    }
+	}
-    wait_for_button_release(BUTTON_B);
+	wait_for_button_release(BUTTON_B);
-    clear();
+	clear();
-    print("TEST555");
+	print("Go!");
+	// Play music and wait for it to finish before we start driving.
+	play_from_program_space(go);
+	while(is_playing());
 }
 // This is the main function, where the code starts.  All C programs
 // must have a main() function defined somewhere.
 int main()
 {
-    unsigned int sensors[5]; // an array to hold sensor values
+	unsigned int sensors[5]; // an array to hold sensor values
-    unsigned int last_proportional=0;
+	unsigned int last_proportional=0;
-    long integral=0;
+	long integral=0;
-    currentIdx = 0;
+	// set up the 3pi
-    // set up the 3pi
+	initialize();
-    initialize();
-    //int val =0;
+	// This is the "main loop" - it will run forever.
-    //char lisa=0;
+	while(1)
-    int x=18;       //prop
+	{
-    int y=0;        //int
+		// Get the position of the line.  Note that we *must* provide
-    int z1=4,z2=1;      //deriv
+		// the "sensors" argument to read_line() here, even though we
-    int max = 150;
+		// are not interested in the individual sensor readings.
-    int butp = 0;
+		unsigned int position = read_line(sensors,IR_EMITTERS_ON);
-    /*  pval = eeprom_read_byte((uint8_t*)10);
-        ival = eeprom_read_byte((uint8_t*)11);
+		// The "proportional" term should be 0 when we are on the line.
-        dval1 =eeprom_read_byte((uint8_t*)12);
+		int proportional = ((int)position) - 2000;
-        dval2 =eeprom_read_byte((uint8_t*)13);*/
-    while(1)
+		// Compute the derivative (change) and integral (sum) of the
-    {
+		// position.
+		int derivative = proportional - last_proportional;
-        if(butp ==0)
+		integral += proportional;
-        {
-            if(button_is_pressed(BUTTON_C))
+		// Remember the last position.
-            {
+		last_proportional = proportional;
-                x++;
-            }
+		// Compute the difference between the two motor power settings,
-            else if(button_is_pressed(BUTTON_A))
+		// m1 - m2.  If this is a positive number the robot will turn
-            {
+		// to the right.  If it is a negative number, the robot will
-                x--;
+		// turn to the left, and the magnitude of the number determines
-            }
+		// the sharpness of the turn.
-            print("Proport: ");
+		int power_difference = proportional/20 + integral/10000 + derivative*3/2;
-            lcd_goto_xy(0,1);
-            print_long(x);
+		// Compute the actual motor settings.  We never set either motor
-            delay_ms(100);
+		// to a negative value.
-        }
+		const int max = 60;
-        else if(butp==1)
+		if(power_difference > max)
-        {
+			power_difference = max;
-            if(button_is_pressed(BUTTON_C))
+		if(power_difference < -max)
-            {
+			power_difference = -max;
-                y++;;
-            }
+		if(power_difference < 0)
-            else if(button_is_pressed(BUTTON_A))
+			set_motors(max+power_difference, max);
-            {
+		else
-                y--;
+			set_motors(max, max-power_difference);
-            }
+	}
-            print("Integral: ");
-            lcd_goto_xy(0,1);
+	// This part of the code is never reached.  A robot should
-            print_long(y);
+	// never reach the end of its program, or unpredictable behavior
-            delay_ms(100);
+	// will result as random code starts getting executed.  If you
-        }
+	// really want to stop all actions at some point, set your motors
-        else if(butp==2)
+	// to 0,0 and run the following command to loop forever:
-        {
+	//
-            if(button_is_pressed(BUTTON_C))
+	// while(1);
-            {
-                z1++;
-            }
-            else if(button_is_pressed(BUTTON_A))
-            {
-                z1--;
-            }
-            print("D ?/Z2: ");
-            lcd_goto_xy(0,1);
-            print_long(z1);
-            delay_ms(100);
-        }
-        else if(butp==3)
-        {
-            if(button_is_pressed(BUTTON_C))
-            {
-                z2++;
-            }
-            else if(button_is_pressed(BUTTON_A))
-            {
-                z2--;
-            }
-            print("D Z1/?: ");
-            lcd_goto_xy(0,1);
-            print_long(z2);
-            delay_ms(100);
-        }
-        else if(butp==4)
-        {
-            if(button_is_pressed(BUTTON_C))
-            {
-                max++;
-            }
-            else if(button_is_pressed(BUTTON_A))
-            {
-                max--;
-            }
-            if(max >= 255)
-            {
-                max = 255;
-            }
-            if(max <= 20)
-            {
-                max = 20;
-            }
-            print("Speed: ");
-            lcd_goto_xy(0,1);
-            print_long(max);
-            delay_ms(50);
-        }
-        else if(butp > 4) break;
-        if(button_is_pressed(BUTTON_B))
-        {
-            butp++;
-            delay_ms(200);
-        }
-        clear();
-    }
-    clear();
-    print("  3Pi");
-    lcd_goto_xy(0,1);
-    print("  118");
-    // This is the "main loop" - it will run forever.
-    while(1)
-    {
-        if (currentIdx < MELODY_LENGTH && !is_playing())
-        {
-            // play note at max volume
-            play_note(note[currentIdx], duration[currentIdx], 15);
-            currentIdx++;
-        }
-        if(currentIdx >= 95) currentIdx = 0;
-        // Get the position of the line.  Note that we *must* provide
-        // the "sensors" argument to read_line() here, even though we
-        // are not interested in the individual sensor readings.
-        unsigned int position = read_line(sensors,IR_EMITTERS_ON);
-        // The "proportional" term should be 0 when we are on the line.
-        int proportional = ((int)position) - 2000;
-        // Compute the derivative (change) and integral (sum) of the
-        // position.
-        int derivative = proportional - last_proportional;
-        integral += 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/x + integral/y + derivative*(z1/z2);
-        // Compute the actual motor settings.  We never set either motor
-        // to a negative value.
-        if(power_difference > max)
-            power_difference = max;
-        if(power_difference < -max)
-            power_difference = -max;
-        if(power_difference < 0)
-            set_motors(max+power_difference, max);
-        else
-            set_motors(max, max-power_difference);
-    }
-    // This part of the code is never reached.  A robot should
-    // never reach the end of its program, or unpredictable behavior
-    // will result as random code starts getting executed.  If you
-    // really want to stop all actions at some point, set your motors
-    // to 0,0 and run the following command to loop forever:
-    //
-    // while(1);
 }
 // Local Variables: **
 // mode: C **
@@ Line 521: / Line 399: @@
 // indent-tabs-mode: t **
 // end: **
 </code>

et/projects/3pi/solutions.1447330943.txt.gz · Last modified: 2020/07/20 09:00 (external edit)