Line follower robot simluation

blyk

I have tried to create a simulation of line follower robot but somehow sometimes it fails to recognize the paths. Can anyone point me where I am doing wrong. Click and draw to create path using mouse. Also try to draw the line from one of those sensors.

Here is the code -

    PGraphics pg;
    boolean left, right;
    void setup() {
      size(400, 400);
      pg = createGraphics(width, height);
      x = width/2;
      y = height/2;
      ang = 0;
      dx = 0;
    } 
    int t=0, ang, R = 10, r=2;
    float x, y, dx, dy;
    void draw() {
      background(-1);
      pg.beginDraw();
      pg.stroke(0);
      pg.strokeWeight(8);
      if (mousePressed)
        pg.line(mouseX, mouseY, pmouseX, pmouseY);
      pg.endDraw();
      image(pg, 0, 0);
      if (left) {
        ang+=3;
        dx =  sin(radians(ang));
        dy =  cos(radians(ang));
        left = false;
      }   
      if (right) {
        ang-=3;
        dx =  sin(radians(ang));
        dy =  cos(radians(ang));
        right = false;
      }
      robot(x, y, ang);
      x+=dx/2;
      y+=dy/2; 
      if (x<0) x = width; 
      if (x>width) x = 0;
      if (y<0) y = height; 
      if (y>height) y = 0;
    }
    void robot(float x, float y, float ang) {
      noFill();
      stroke(0, 50);
      ellipse(x, y, 2*R, 2*R);
      noStroke();
      fill(0, 50);
      ellipse(x, y, 2*r, 2*r);
      for (int i=0; i<2; i++) {
        float xx = x + R*sin(i*PI+radians(ang)+PI/2); 
        float yy = y + R*cos(i*PI+radians(ang)+PI/2);
        ellipse(xx, yy, 2*r, 2*r);
        color c = pg.get((int)xx, (int)yy);
        if (i==0 && c!=0 ) left = true;
        if (i==1 && c!=0 ) right = true;
      }
    }

TfGuy44

Nice problem. Good code posting. No idea what's wrong. Will have a look at it more tonight. Posting here so I don't lose this thread. :-)

Chrisir

I like it.

but he is confused.

TfGuy44

Seems a little happier now. Might be because I reordered the way in which the bot decides to do things. Do you still see any line-ignoring behavior?

PGraphics pg;
LineBot robbie;

void setup() {
  size(400, 400);
  pg = createGraphics(width, height);
  pg.beginDraw();
  pg.clear();
  pg.endDraw();
  robbie = new LineBot();
} 

void draw() {
  background(255);
  if (mousePressed) {
    pg.beginDraw();
    pg.stroke(0);
    pg.strokeWeight(8);
    pg.line(mouseX, mouseY, pmouseX, pmouseY);
    pg.endDraw();
  }
  image(pg, 0, 0);
  robbie.draw();
}



class LineBot {
  final static int R=10, r=2;
  float x, y, ang;
  float dx=0, dy=0;
  boolean left, right;
  LineBot() {
    x=width/2;
    y=height/2;
    ang=0;
  }
  void draw() {
    simulate();
    render();
  }
  void simulate() {
    // Move.
    x+=dx/2;
    y+=dy/2; 
    if (x<0) x = width; 
    if (x>width) x = 0;
    if (y<0) y = height; 
    if (y>height) y = 0;
    // Check detectors.
    for (int i=0; i<2; i++) {
      float xx = x + R*sin(i*PI+radians(ang)+PI/2); 
      float yy = y + R*cos(i*PI+radians(ang)+PI/2);
      ellipse(xx, yy, 2*r, 2*r);
      color c = pg.get((int)xx, (int)yy);
      if (i==0 && c!=0 ) left = true;
      if (i==1 && c!=0 ) right = true;
    }
    // Turning,
    if (left) {
      ang+=3;
      dx =  sin(radians(ang));
      dy =  cos(radians(ang));
      left = false;
    }   
    if (right) {
      ang-=3;
      dx =  sin(radians(ang));
      dy =  cos(radians(ang));
      right = false;
    }
  }
  void render() {
    // Drawing.
    noFill();
    stroke(0, 50);
    ellipse(x, y, 2*R, 2*R);
    noStroke();
    fill(0, 50);
    ellipse(x, y, 2*r, 2*r);
  }
}

blyk

Thanks @TfGuy44 , It improved the code but it was still loosing the line . ... sorry :)

So I took the stab and changed the line 52-53 in my code with

float xx = x + R*sin(i*3*PI/2+radians(ang)+PI/2); 
float yy = y + R*cos(i*3*PI/2+radians(ang)+PI/2);

And it worked. I guess the problem was the sensor were way too apart from each other :)