• fullscreen
  • Coord.pde
  • Explorer.pde
  • Mundo.pde
  • Robot.pde
  • public class Coord {
      float px;
      float py;
      int plx; 
      int ply;
      int estado;
      public float rt=0, gt=0, bt=0, r=0, g=0, b=0;
      float altura2 = 28;
    
      public Coord(float px, float py, int plx, int ply) {
        this.px = px;
        this.py = py;
        this.plx = plx;
        this.ply = ply;
        estado = 0;
      }
    
      public void setEstado(int estado) {
        this.estado = estado;
      }
    
      public boolean visitable() {
        return (estado >= 0 );
      }
    
      public void setColor() {
    
        if (estado==-2) {
          rt = 0;
          gt = 255;
          bt = 0;
        }
        else if (estado==-3){
          rt = 255;
          gt = 0;
          bt = 0;
        }
        else {
          rt = 0;
          gt = 0;
          bt = 250;
        }
    
    
    
        float ease = .05;
        r+=(rt-r)*ease;
        g+=(gt-g)*ease;
        b+=(bt-b)*ease;
        fill(r, g, b);
      }
    
    
      public void dibujar() {
    
    
        float altura = 0;
        if (estado>=-2) {
          altura = b/255*25;
          marca();
        } 
        else   if (estado==-3) {
          altura = altura2;
    
        } 
        setColor();
        pushMatrix();
        translate(px,  py);
        //stroke(.5);
        noStroke();
        box(35, 35, altura);
        popMatrix();
      }
    
      public void marca() {
        pushMatrix();
        translate(px,  py, 1);
        fill(255);
        rect(0, 0, 10, 10);
        popMatrix();
      }
    }
    
    
    
    
    
    
    //import processing.opengl.*;
    
    Robot r2d2;
    float rotacionT = .3;
    float rotacion  = 0;
    float px, py;
    
    void setup() {
      size (650, 650, P3D);
      frameRate(200);
      rectMode(CENTER);
      noStroke();
      lights();
      r2d2 = new Robot();
    }
    
    
    void draw() {
      camera(width/2.0 + px/2, height/2.0 + py/2, (height/2.0) / tan(PI*60.0 / 360.0), width/2.0 + px, height/2.0 + py, 0, 0, 1, 0);
      
      
      px += (r2d2.px - px)*.02;
      py += (r2d2.py - py)*.02;
      if (rotacion<rotacionT-.01) {
        rotacion += (rotacionT-rotacion)*.006;
      }
    
      background(0);
      translate(width/2, height/2-25, 210);
      rotateX(rotacion);
      pointLight(255, 255, 255, px, py, 50);
      //spotLight(1, 1, 1, px, py, 0, 0, 0, -1, 0, 100);
      r2d2.m1.draw();
      r2d2.draw();
      r2d2.pensar();
    }
    
    
    
    
    public class Mundo {
      Coord[][] grid;
      int size;
      public float lado = 35;
    
      public Mundo(int size) {
        this.size = size;
        grid = new Coord[size][size];
        //ellipseMode(CORNER);
        for (int x = 0; x<grid.length; x++) {
          for (int y=0; y<grid[x].length; y++) {
            grid[x][y] = new Coord( (x- grid.length/2)*lado, (y - grid.length/2)*lado, x, y);
            grid[x][y].setEstado(1);
            if (random(0, 100)>75) {
              grid[x][y].setEstado(-3);
            }
          }
        }
    
      } 
    
      public void draw() {
        for (int i=0; i<grid.length; i++) {
          for (int j=0; j<grid.length; j++) {
            grid[i][j].dibujar();
          }
        }
       
      }
    
      public Coord getOrigen() {
        grid[0][7].setEstado(2);
        return grid[0][7];
      }
    
      public float getLado() {
        return lado;
      }
    
      public Coord getDerecha(int cx, int cy) {
        if (cx<size-1) {
          return  grid[cx+1][cy];
        }
        else return null;
      }
    
      public Coord getIzquierda(int cx, int cy) {
        if (cx>0) {
          return  grid[cx-1][cy];
        }
        else return null;
      }
    
      public Coord getAbajo(int cx, int cy) {
        if (cy<size-1) {
          return  grid[cx][cy+1];
        }
        else return null;
      }
    
      public Coord getArriba(int cx, int cy) {
        if (cy>0) {
          return  grid[cx][cy-1];
        }
        else return null;
      }
    
    
    
    }
    
    
    
    
    
    public class Robot{
      Mundo m1 = new Mundo(15);
      Coord raiz;
      Coord actual;
      Coord target;
      Stack anteriores;
    
      float rotacion = HALF_PI;
      int estado = 0;
      float px, py;
      float diametro = m1.getLado();
      float radio = m1.getLado()/2;
    
    
      public Robot() {
        raiz = m1.getOrigen();
        target = null;
        px = raiz.px;
        py = raiz.py;
        actual = raiz;
        anteriores = new Stack();
      }
    
      public void draw() {
        //Llantas
        pushMatrix();
        translate(px, py, 0);
        rotateZ(rotacion);
        fill(255, 255, 0);
        scale(1, .6, 1);
        box(m1.lado);
        
        fill(0, 100, 0);
        scale(1, 1/.6, 1);
        translate(8, 0, 0);
        scale(.3, .8, .9);
        box(m1.lado);
        translate(-60, 0, 0);
        box(m1.lado);
        
        popMatrix();
     
      }
    
      public void pensar() {
        if (estado==0) {
    
          if (sensorDerecho()!=null && sensorDerecho().visitable()) {
            target = sensorDerecho();
            actual.setEstado(-2);
            estado = 1;
            anteriores.push(actual);
            rotacion = 0;
          }
          else  if (sensorAbajo()!=null && sensorAbajo().visitable()) {
            target = sensorAbajo();
            actual.setEstado(-2);
            estado = 1;
            anteriores.push(actual);
            rotacion = HALF_PI;
          }
          else  if (sensorIzquierda()!=null && sensorIzquierda().visitable()) {
            target = sensorIzquierda();
            actual.setEstado(-2);
            estado = 1;
            anteriores.push(actual);
            rotacion = PI;
          }
          else  if (sensorArriba()!=null && sensorArriba().visitable()) {
            target = sensorArriba();
            actual.setEstado(-2);
            estado = 1;
            anteriores.push(actual);
            rotacion = HALF_PI + PI;
          }
          else {
            if (anteriores.size()>0){
              target = (Coord) anteriores.pop();
              if (target.plx == actual.plx && target.ply>actual.ply) {
                rotacion = HALF_PI;
              }
              else if (target.plx == actual.plx && target.ply<actual.ply) {
                rotacion = HALF_PI+PI;
              }
              else if (target.ply == actual.ply && target.plx<actual.plx) {
                rotacion = PI;
              }
              else if (target.ply == actual.ply && target.plx>actual.plx) {
                rotacion = 0;
              }
              actual.setEstado(-2);
              estado = 1;
            }
          }
    
        }
    
        if (estado==1) {
          float dx = target.px - px;
          float dy = target.py - py;
          float dis = sqrt(dx*dx + dy*dy);
    
          px += dx*.25;
          py += dy*.25;
    
    
          if (dis<=.1) {
            actual = target;
            estado = 0;
            target = null;
          }
    
        }
      }
    
    
      public Coord sensorDerecho() {
        return m1.getDerecha(actual.plx, actual.ply);
      } 
      public Coord sensorIzquierda() {
        return m1.getIzquierda(actual.plx, actual.ply);
      } 
      public Coord sensorArriba() {
        return m1.getArriba(actual.plx, actual.ply);
      } 
      public Coord sensorAbajo() {
        return m1.getAbajo(actual.plx, actual.ply);
      } 
    
    
    }
    
    
    
    
    
    
    
    

    code

    tweaks (0)

    about this sketch

    This sketch is running as Java applet, exported from Processing.

    license

    advertisement

    Marco Alejandro

    Path Finder Robot AI

    Add to Faves Me Likey@! 1
    You must login/register to add this sketch to your favorites.

    Simple Path Finder Robot AI

    i have modified ur code so it can take data from serial and draw according to it not just random blocks and i hope to communicate with u.
    Marco Alejandro
    17 Jun 2013
    Nice to hear that Mostafa! Im glad its useful to someone. You can contact me at ----&gt; alexandre.macedonio(AT)gmail.com. Regards!
    WoW,I coud not expect fast response like that and i recently applied it on real robot as a scanner robot to scan an area and build a map according to ultrasonic readings,you dont know how your code really helped me. GRRRRRRRREEEETTT THANKS
    I would like to add an algorithm such that after finish when mouse pressed in certain coord the robot choose the shortest path to this coord and move into it any suggestion would be helpful.
    Marco Alejandro
    18 Jun 2013
    Mostafa I'm not sure I get what you need clearly. The current algorithm does not stores the relationship between each point, the search method actually does not care about the shortest path but rather the completition of the complete map.
    In order to build a shortest path you could use a Dijkstra prim algorithm. You would have to know the relationship between each point on the map, ie from point A I can go to B C D or C with a measured / aprox distance between each node:
    http://en.wikipedia.org/wiki/Prim's_algorithm
    Thanks for help but i want to find shortest path such that if the mouse pressed in certain position in the map first i get that (mouseX and mousY) related to which plx and ply (grid element) then the robot should detect the path(s) to this grid element such that it choose the minimum number of grid elements it will travel to reach the required position which is considered the shortest path i hope i get my point clear .thanks again for ur feedback.
    And, all of this will be done after the scan of closed area has been done so ,we will have grid elements which we can't visit .So the algorithm calculate number of blocks to travel by different possible ways(available paths) to reach required position considering the non visitable blocks then choose minimum number of blocks to travel (which is the shortest path) sorry for disturbing you and my bad english.
    You need to login/register to comment.