localisation


#1

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] #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);
}

}}
}}

[/code]