localisation

The Easy Robot for Everyone!

localisation

Postby ashbot » Sat Apr 25, 2015 8:35 pm

Hi guys
I was wondering if anyone could help me with a piece of code I am working on? Do you think this is an example of Sparki's localisation abilities? If not what can I do to alter it accordingly? The program itself works really well as long as you start the Sparki with the landmark on the right...the ultrasonic ranger can have difficulties sometimes.

oh I should also explain this program is written to take place inside a rectangular area with outer walls so the Sparki can position himself in relation to them, and a starting "landmark" to set the heading correctly. The home zone comes inside a 9, 9 area and the waypoint is the same but with a different heading.

Thanks in advance

Ash

Code: Select all
 #include <Sparki.h> // include the sparki library   

void setup()
{
    sparki.servo(0);
  sparki.clearLCD();
  sparki.updateLCD();
  delay(2000);
    int landmark=0;
    int heading1=0;
    int heading2=heading1-90;
  sparki.servo(0);
  heading1=0;
  delay(2000);
  sparki.ping();
  if (sparki.ping () < 7)
  {
    landmark=sparki.ping();
    sparki.print("landmark= ");
    sparki.println(landmark);
    sparki.updateLCD();
    delay(2000);
    sparki.moveLeft(90);
  }
  sparki.servo(85);
  delay(2000);
  sparki.ping();
  if ((sparki.ping() < 7) && (sparki.ping() > 1))
  {
    landmark=sparki.ping();
    sparki.print("landmark= ");
    sparki.println(landmark);
    sparki.updateLCD();
    delay(2000);
    int heading1=0;
     sparki.print("heading1=");
  sparki.println(heading1);
  sparki.updateLCD();
    delay(2000);
  }
  else
  if (sparki.ping()>7)
  {
    sparki.servo(-85);
    delay(2000);
    sparki.ping();
  if (sparki.ping() < 7)
  {
    landmark=sparki.ping();
    sparki.print("landmark= ");
    sparki.println(landmark);
    sparki.updateLCD();
    delay(2000);
    sparki.moveBackward(8);
    sparki.moveRight(270);
    sparki.moveForward(10);
    sparki.moveRight(90);
    sparki.moveForward(8);
    sparki.servo(85);
  delay(2000);
  sparki.ping();
  if (sparki.ping() < 7)
  {
    landmark=sparki.ping();
    sparki.print("landmark= ");
    sparki.println(landmark);
    sparki.updateLCD();
    delay(2000);
    int heading1=0;
     sparki.print("heading1=");
  sparki.println(heading1);
  sparki.updateLCD();
    delay(2000);
  }}}
}
void loop()
{
  int roomA=0;
  int roomB=0;
  int roomC=0;
  int roomD=0;
  int roomE=0;
  int roomF=0;
  int positionA=0;
  int positionB=0;
  int positionC=0;
  int positionD=0;
  int positionE=0;
  int positionF=0;
  int heading1=0;
  int heading2=heading1-90;
  int heading3=heading1+90;
  int heading4=heading1+180;
  int Home= ((positionA<9) &&(positionC<9) && (heading2=-90));
  int x=positionA-8;
  int y=positionB-8;
  int x1=positionD-8;
  int y1=positionE-8;
  int moveTo1=x;
  int moveTo2=y;
  int moveTo3=x1;
  int moveTo4=y1;
  int waypoint= ((positionD<9)) && ((positionF<9)) && ((heading4=180));

  sparki.servo(0);
  delay(2000);
  roomA=sparki.ping();
  sparki.print("roomA= ");
  sparki.println(roomA);
  sparki.updateLCD();
  delay(2000);
  positionA=(roomA-4);
   sparki.print("positionA= ");
  sparki.println(positionA);
  sparki.updateLCD();
  delay(2000);
  sparki.servo(-85);
   delay(2000);
  sparki.ping();
  roomB=sparki.ping();
  sparki.print("roomB=");
  sparki.println(roomB);
  sparki.updateLCD();
  delay(2000);
  positionB=(roomB-4);
   sparki.print("positionB= ");
  sparki.println(positionB);
  sparki.updateLCD();
  delay(2000);
     sparki.print("heading1=");
  sparki.println(heading1);
  sparki.updateLCD();
    delay(2000);
  if(positionA >9)
  {
    sparki.servo(0);
    int x= positionA-8;
    int moveTo1= x;
    sparki.print("moveTo1=");
    sparki.println(moveTo1);
    delay(1000);
    sparki.moveForward(moveTo1);
  }
  sparki.servo(-85);
   delay(2000);
  sparki.ping();
  roomB=sparki.ping();
  sparki.print("roomB=");
  sparki.println(roomB);
  sparki.updateLCD();
  delay(2000);
  positionB=(roomB-4);
   sparki.print("positionB= ");
  sparki.println(positionB);
  sparki.updateLCD();
  delay(2000);

     sparki.print("heading1=");
  sparki.println(heading1);
  sparki.updateLCD();
    delay(2000);
  if((positionA<9)&&(positionB>9))
  {
     int heading2  = heading1-90;
  sparki.print("heading2=");
  sparki.println(heading2);
  sparki.updateLCD();
    delay(2000);
    sparki.moveLeft(90);
     delay(1000);
    sparki.servo(0);
    int y=positionB-8;
    int moveTo2= y;
    sparki.print("moveTo2=");
    sparki.println(moveTo2);
    delay(1000);
    sparki.moveForward(moveTo2);
    sparki.servo(0);
  delay(2000);
  sparki.ping();
  roomA=sparki.ping();
  sparki.print("roomA= ");
  sparki.println(roomA);
  sparki.updateLCD();
  delay(2000);
  positionA=(roomA-4);
   sparki.print("positionA= ");
  sparki.println(positionA);
  sparki.updateLCD();
  delay(2000);
  sparki.servo(85);
   delay(2000);
  sparki.ping();
  roomC=sparki.ping();
  sparki.print("roomC=");
  sparki.println(roomC);
  sparki.updateLCD();
  delay(2000);
  positionC=(roomC-4);
   sparki.print("positionC= ");
  sparki.println(positionC);
  sparki.updateLCD();
  delay(2000);
  int Home= ((positionA<9) &&(positionC<9) && (heading2=-90));
  if (Home);
  {
    for (int i = 0; i < 3; i=i+1)
    {
      sparki.beep(740, 600);
  delay(400);
  sparki.beep(540, 400);
  delay(400);
  sparki.beep(540, 200);
  delay(200);
  sparki.beep(440, 200);
  delay(200);
  sparki.beep(640, 400);
  delay(400);
  sparki.beep(740, 200);
  delay(400);
  sparki.beep(740, 200);
    }
    delay(1000);
    int Home= ((positionA<9) &&(positionB<9) && (positionC<9) &&(heading2=-90));
     sparki.print("Home");
  sparki.println(Home);
  sparki.updateLCD();
  delay(3000);
 
  }
  sparki.moveLeft(180);
  sparki.servo(0);
   delay(2000);
  sparki.ping();
  roomD=sparki.ping();
  sparki.print("roomD=");
  sparki.println(roomD);
  sparki.updateLCD();
  delay(2000);
  positionD=(roomD-4);
   sparki.print("positionD= ");
  sparki.println(positionD);
  sparki.updateLCD();
  delay(2000);
 sparki.servo(85);
   delay(2000);
  sparki.ping();
  roomE=sparki.ping();
  sparki.print("roomE=");
  sparki.println(roomE);
  sparki.updateLCD();
  delay(2000);
  positionE=(roomE-4);
   sparki.print("positionE= ");
  sparki.println(positionE);
  sparki.updateLCD();
  delay(2000);
     sparki.print("heading3=");
  sparki.println(heading3);
  sparki.updateLCD();
    delay(2000);
    if (positionD>9)
    {
      sparki.servo(0);
    int x1=positionD-8;
    int moveTo3=x1;
    sparki.print("moveTo3=");
    sparki.println(moveTo3);
    delay(2000);
    sparki.moveForward(moveTo3);
  sparki.servo(85);
   delay(2000);
  sparki.ping();
  roomE=sparki.ping();
  sparki.print("roomE=");
  sparki.println(roomE);
  sparki.updateLCD();
  delay(2000);
  positionE=(roomE-4);
   sparki.print("positionE= ");
  sparki.println(positionE);
  sparki.updateLCD();
  delay(2000);
 
    if (positionE>9)
    {
      sparki.moveRight(90);
      int heading4= heading1+180;
       sparki.print("heading4=");
  sparki.println(heading4);
  sparki.updateLCD();
    delay(2000);
    int y1=positionE-8;
    int moveTo4=y1;
    sparki.print("moveTo4=");
    sparki.println(moveTo4);
    delay(2000);
    sparki.moveForward(moveTo4);
    sparki.servo(0);
   delay(2000);
  sparki.ping();
  roomD=sparki.ping();
  sparki.print("roomD=");
  sparki.println(roomD);
  sparki.updateLCD();
  delay(2000);
  positionD=(roomD-4);
   sparki.print("positionD= ");
  sparki.println(positionD);
  sparki.updateLCD();
  delay(2000);
 sparki.servo(-85);
   delay(2000);
  sparki.ping();
  roomF=sparki.ping();
  sparki.print("roomF=");
  sparki.println(roomF);
  sparki.updateLCD();
  delay(2000);
  positionF=(roomF-4);
   sparki.print("positionF= ");
  sparki.println(positionF);
  sparki.updateLCD();
  delay(2000);
  sparki.print("heading4=");
  sparki.println(heading4);
  sparki.updateLCD();
    delay(2000);
  int waypoint= ((positionD<9)) && ((positionF<9)) && ((heading4=180));
    if (waypoint)
    {
      for (int i = 0; i < 3; i=i+1)
      {
  sparki.RGB(RGB_RED);
  delay(500);

  sparki.RGB(RGB_ORANGE);
  delay(500);
 
  sparki.RGB(RGB_YELLOW);
  delay(500);
 
  sparki.RGB(RGB_GREEN);
  delay(500);
 
  sparki.RGB(RGB_BLUE);
  delay(500);
 
  sparki.RGB(RGB_INDIGO);
  delay(500);

  sparki.RGB(RGB_VIOLET);
  delay(500);

  sparki.RGB(RGB_WHITE);
  delay(500);
 
  sparki.RGB(RGB_OFF);
  delay(500);
}
delay(1000);
    int waypoint= ((positionD<9)) && ((positionF<9)) && ((heading4=180));
     sparki.print("waypoint");
  sparki.println(waypoint);
  sparki.updateLCD();
  delay(3000);
    }
     

 
}}
  }}
   
   
   
   
ashbot
 
Posts: 37
Joined: Thu Nov 20, 2014 11:08 pm

Return to Sparki

Who is online

Users browsing this forum: Yahoo [Bot] and 5 guests

cron