Code - LookAtClosest

Sparki
#1

Mods, How about a sub-forum just for people to upload their code?

24 hours since I got it and really enjoying my Sparki!

I wrote a routine to have the range finder scan 180 degrees and return the angle of the closest detected object. Demo program does the scan, points the range finder in the direction found and shows angle & distance on the LCD, waits 10 seconds and loops.

I can see the routine used in steering or as a way to start other actions when something comes within range, maybe as a kind of burglar alarm.

The other obvious adaptation would be to change the routine to find the direction with the most distant object.

No copyrights claimed, use or adapt as you like with or without attribution.

[code]/*
LookAtClosest by Zax

Demo of routine to determine angle of closest
object the Ultrasonic Range Finder can detect

Program scans for closest, points
the servo in that direction and prints
angle & distance on the LCD
*/

#include <Sparki.h>

void setup()
{
sparki.clearLCD();
}
void loop()
{
// Call routine to find angle of nearest object
int clsdir = closestDir();
// Turn servo to angle
sparki.servo(clsdir);
// Print angle on LCD
sparki.print(“Angle:”);
sparki.println(clsdir);
// Find distance
int cm = sparki.ping();
// Print distance
sparki.print(“Distance:”);
sparki.println(cm);
sparki.updateLCD();
// Wait 10 seconds
delay(10000);

}

int closestDir()
/* Routine to find the angle that has the
nearest object Sparki can "see"
As the servo rotates, the RGB LED will
light if the current distance is the closest

*/
{
// Initialize closest distance
// you could set this low to ignore
// objects farther away
int closest = 999;
// Initialize direction - this will be
// returned if nothing is closer than
// closest, set above
int dir = 0;

// step the servo throgh the angles
for(int angle = -90; angle < 91; angle = angle + 10)
{
//set servo angle
sparki.servo(angle);
//delay(50);

 //read distance
 int cm = sparki.ping();

 // check if distance can be read
 if (cm != -1)
 {
    //Is this the closest yet?
    if (cm < closest)
    {
      //set this as closest abgle
      dir = angle;
      // update closest distance
      closest = cm;
      // light RGB as indicator new closest
      //has been found
      sparki.RGB(RGB_BLUE);
    }
    else
    {
      // Not the closest - turn off RGB
      sparki.RGB(RGB_OFF);          
    }
 }

}
return dir;
}[/code]

#2

Great idea and well done. I got my Sparki yesterday too :smiley:

#3

Nice! We’ll be rolling an a sweep function like this soon that the people at RoboRealm wrote for us.

#4

Have been trying to figure out how I would get sparki to travel to the nearest object. This program seems to be in the right direction, but not sure how to incorporate traveling to nearest object. Anyone have any idea?

#5

with
clsdir as the direction returned by the function
and angleAdj as the adjustment factor for Sparki’s angles
this should turn Sparki in the proper direction

if (clsdir < 0) { sparki.moveLeft(clsdir*-1 * angleAdj); } else { sparki.moveRight(clsdir * angleAdj); }

I also suggest you look at Craig’s Find and Pick up objects sketch in this thread [url]Sample code - Find and pickup objects]

#6

Zax thanks for that. I modified your code to combine both the find closest object, then go fetch it & put it somewhere else. Here it is:

[code]/*
/*
LookAtClosest by Zax
Modified by Trevor - 27th Feb 2014.

Demo of routine to determine angle of closest
object the Ultrasonic Range Finder can detect

Program scans for closest, points
the servo in that direction and prints
angle & distance on the LCD

It then goes & fetches it & drops it in a different location.
*/

#include <Sparki.h>

void setup()
{
sparki.clearLCD();
}

void loop()
{
sparki.gripperOpen();
// Call routine to find angle of nearest object
int clsdir = closestDir();
// Turn servo to angle
sparki.servo(clsdir);
// Print angle on LCD
sparki.print(“Angle:”);
sparki.println(clsdir);
// Find distance
int cm = sparki.ping();
// Print distance
sparki.print(“Distance:”);
sparki.println(cm);
sparki.updateLCD();

//flash the lights!!!
sparki.RGB(RGB_BLUE);
delay(500);
sparki.RGB(RGB_RED);
delay(500);
sparki.RGB(RGB_GREEN);
delay(500);
sparki.RGB(RGB_ORANGE);
delay(500);
sparki.RGB(RGB_WHITE);
delay(500);
sparki.RGB(RGB_VIOLET);
delay(500);
sparki.RGB(RGB_PINK);
delay(500);
sparki.RGB(RGB_INDIGO);
delay(500);
sparki.RGB(RGB_OFF);
}

int closestDir()
/* Routine to find the angle that has the
nearest object Sparki can "see"
As the servo rotates, the RGB LED will
light if the current distance is the closest

*/
{
// Initialize closest distance
// you could set this low to ignore
// objects farther away
int closest = 999;
// Initialize direction - this will be
// returned if nothing is closer than
// closest, set above
int dir = 0;

// step the servo throgh the angles
for(int angle = -90; angle < 91; angle = angle + 10)
{
//set servo angle
sparki.servo(angle);
//delay(50);

 //read distance
 int cm = sparki.ping();

 // check if distance can be read
 if (cm != -1)
 {
    //Is this the closest yet?
    if (cm < closest)
    {
      //set this as closest angle
      dir = angle;
      // update closest distance
      closest = cm;
      // light RGB as indicator new closest
      //has been found
      sparki.RGB(RGB_BLUE);
    }
    else
    {
      // Not the closest - turn off RGB
      sparki.RGB(RGB_OFF);          
    }
 }

}
//return dir;

//Turn towards object.
int clsdir = dir;
int angleAdj = 1;

if (clsdir < 0)
{
sparki.moveLeft(clsdir*-1 * angleAdj);
}

    else
    {
      sparki.moveRight(clsdir * angleAdj);         
    }
      
      sparki.RGB(RGB_RED); //turn on red light
      sparki.servo(0); //center servo - just to look pretty :)
      sparki.moveForward(closest+5); // move to object
      sparki.gripperClose(); //pick it up.
      delay (5000); //wait 5 seconds
      sparki.gripperStop(); //then stop gripper closing - stop servo grinding.
      
      //Move to another location & drop it.
      sparki.moveBackward(10);
      sparki.moveRight(110);
      sparki.moveForward(30);
      sparki.gripperOpen();
      sparki.RGB(RGB_GREEN);
      
      //Turn 180º & move away.
      sparki.moveBackward(10);
      sparki.moveLeft(180);
      sparki.moveForward(50);
      sparki.moveRight(180);

}

[/code]

#7

Looks good Trevor.

angleAdj should really be a float instead of an int. I think people have said to use 1.055 for the adjustment factor. if you find your angles are off a bit you could try it.

#8

[quote=“Zax”]Looks good Trevor.

angleAdj should really be a float instead of an int. I think people have said to use 1.055 for the adjustment factor. if you find your angles are off a bit you could try it.[/quote]

Ah, thanks. Wasn’t sure. I defaulted to 1. Close, but misses sometimes. I’ll change it to 1.055. Cheers.

#9

Thank for sharing the code…by the way what are the values you use for the other RGB colors? You must have had them defined in “sparki.h”.

#10

Yep, they were defined by ArcBotics on a previous update to Sparki.h

#define RGB_RED 100, 0, 0
#define RGB_ORANGE 90, 100, 0
#define RGB_YELLOW 60, 100, 0
#define RGB_GREEN 0, 100, 0
#define RGB_BLUE 0, 0, 100
#define RGB_PINK 90, 0, 100
#define RGB_INDIGO 20, 0, 100
#define RGB_VIOLET 60, 0, 100
#define RGB_WHITE 60, 100, 90
#define RGB_OFF 0, 0, 0

#11

Much appreciated. I download and use the installer always. I also scrutinized both sparki .cpp and .h file and have not seen these values defined. Could it be that this was only done in the Github version? Or does ArcBotics has issue tracking the files on their releases?

Anyways, thanks!!!