Home Made 3pi

Hello,
My name is Marius and after i bought a 3pi robot i decided to build one myself for a line followers competition that will take place next year, the problem is that i have to make the robot to do a small drift and then stop at the end of the line where is a 10X10 cm square and i havent been able to do it so far,so it would be awesome if you could help me.

Thanks!

Hi Marius,

There are plenty of code examples for the 3pi that deal with line-following and other related things. Can you show us what code you have, so far?

Geoff

This is the code i have been using so far.

#include <pololu/orangutan.h>
  
unsigned char qtr_rc_pins[] = {IO_C0, IO_C1, IO_C2, IO_C3, IO_C4, IO_C5};
unsigned char qtr_rc_count  = 6;       
unsigned int  qtr_rc_values[6] = {0};


float KP = 3 ,KI = 50000 , KD = 16/1;

int   integral  = 0;
int   last_proportional = 0;

void follow()
{
    int position = qtr_read_line(qtr_rc_values, QTR_EMITTERS_ON);

    int center = (( (qtr_rc_count - 1) * 1000) / 2);

    int proportional = position - center;

    int derivative = proportional - last_proportional;

    int power_difference = proportional / KP + integral / KI + derivative * KD;
    last_proportional    = proportional;
    integral  += proportional;


    const int max = 200;
    const int max_diffrence = 20;
    const int factor_diffrence = 2;

    if(power_difference > max)
        power_difference = max;
    if(power_difference < -max)
        power_difference = -max;

    
    int leftMotorSpeed  = max;
    int rightMotorSpeed = max-power_difference;

    if(power_difference < 0)
    {
        leftMotorSpeed  = max+power_difference;
        rightMotorSpeed = max;
    }


    if(leftMotorSpeed - rightMotorSpeed > max_diffrence)
    {
        leftMotorSpeed -= (leftMotorSpeed - rightMotorSpeed)/factor_diffrence;
    } 
    else if(rightMotorSpeed - leftMotorSpeed > max_diffrence)
    {
        rightMotorSpeed -= (rightMotorSpeed - leftMotorSpeed)/factor_diffrence;
    }

    set_motors(leftMotorSpeed,rightMotorSpeed);

}



int main()
{

    set_digital_input(IO_D4, PULL_UP_ENABLED);

    while(is_digital_input_high(IO_D4)) {}
    qtr_rc_init(qtr_rc_pins,qtr_rc_count, 2000, 255);  

    red_led(1);
    delay(1000);

    int i;
      for (i = 0; i < 100; i++)  
      {
        if(i%25==0)
        {
            if((i/5)%2==0) set_motors(20,-20); 
                else set_motors(-20,20); 
        }

        if(i%5)
        {
            if((i/5)%2==0) red_led(0);
                else red_led(1); 
        }
        qtr_calibrate(QTR_EMITTERS_ON);
        delay(20);
      }
 
    set_motors(0,0); 
    red_led(1);

    while(is_digital_input_high(IO_B0)) {} 
    delay(1000);

    while(1)
    {
       
        follow();
    }

}

I get the sense that the code you posted is fairly complete in terms of basic line-following (correct me if I’m wrong, and if so, please describe what the robot is doing vs. what it should be doing) and so I assume this quoted bit describes the part you are struggling with. Unfortunately, I’m not quite sure I understand what you’re trying to do. Could you explain a bit further, please? (Are you simply trying to make the robot stop when it reaches a large region of electrical tape?)

Geoff

Right now the robot is following the line very good and i need to add the stopping part but i didnt really understand how to do this from the information i found on the internet about the orangutan libraries.So all i have left to do is make the robot stop when it reaches a large region of tape.My first ideea was to write a ‘if(all sensors are on the line)’ and read the sensors to make it stop.

This is exactly the approach you want to use. You might consider some code like the following, which, with some small modifications specific to your program/purposes, will provide the effect you are looking for.


//...
const int STOP_THRESHOLD = 1600;
//...

if(sensors[0] > STOP_THRESHOLD &&
   sensors[1] > STOP_THRESHOLD &&
   sensors[2] > STOP_THRESHOLD &&
   sensors[3] > STOP_THRESHOLD &&
   //...and so on
   )
{
    print("Done!");
    set_motors(0, 0);
    while(1);
}

//...

Geoff

Thank you very much four your help, everithing is ok now.