Here is the code for this project - sorry it is not too polished, but hopefully you will get the general idea of my approach. Apologies for functions not actually used in the project - these were used during my experimentation of different solutions so left in for posterity!
// Object Tracking - measure and report on an objects position using the Loc8tor. The assembly
// will move, scanning first in the x, y plane to determine the strongest signal direction and then
// in the z plane to determine vertical position. The coordinates and distance to the object
// are then outputed to an LCD display.
//
// There are 3 main components to this project:
// 1. the interface to the Loc8tor device
// 2. the motor shield and stepper motors
// 3. the interface to the LCD display
//
// 1. the Loc8tor device (www.loc8tor.co.uk) is a small credit card sized device that allows you to
// track small tags that you attach to items you don't want to misplace like your mobile phone or pet.
// Once the device starts scanning it makes an audible sound that increases in pitch as you approach
// the tag, thus allowing you to incrementally track down and locate the tag.
//
// For this project I took the device apart and made a number of modifications:
// - added two resister to act as a voltage divider to bring voltage from 5v to 3v that the device uses
// - replaced 2 tactile switches with transistors: one to power the device on and off, and one to
// switch the scanning mode on and off
// - removed the inbuilt piezo sounder and added a wire to take the piezo sounder voltage to
// an Arduino input
// so we have 5 wires from the device, power switch, scan switch, piezo feed, +ve and GRD.
// So in brief, I use stepper motors to move the device around and measure the relative pitch
// to locate the direction and estimate distance to the object.
//
// 2. The motor shield is the Adafruit (www.adafruit.com) kit that you need to solder up yourself.
// This allows 2 steppers motors to be attached which I've configured so that one motor sits on
// top of the other at 90 degrees. This allows movement of the entire assemblage in an x, y plane
// and then movement in the z plane at a particular x, y location. The MICROSTEP mode is choosen to
// make the motion as smooth as possible.
//
// 3. With the motor shield attached you only have 8 official Arduino ports you can use. The Loc8tor
// needs 3 (+ve and GRD go to different pins so we are ok there) and 6 are needed for a standard LCD in 4 bit mode.
// (ie. 4 for data, 1 for RS and 1 for Enable). Mine is a jhd162a - thanks to Suhas's (iamsuhasm.wordpress.com)
// for getting be going on this. So I had a a bit of a problem: 9 ports required with only 8
// available. I therefore decided to use a shift register (mine is a 74HC595) as I read you can drive an LCD
// with only 3 pins doing this. I did try some of the 3-pin LCD libraries but didn't get much success with my type
// of shift register so I decided to implement a logical port extender so with just the 3 inputs for the latch, clock and data,
// I simulate 8 ports on the data pins. I then made some modifications to the LiquidCrystal library to call my own version
// of digitalWrite() - this was the least intrusive method of modifying this library; so when LiquidCrystal
// thinks it's writing out to a pin, it will call my function that will shift out 8 bits to the register with the relevant
// bit set to 1 or 0 corresponding to the request to set HIGH and LOW respectively.
// Paul Hampton timegalore@hotmail.com
#include <LiquidCrystal.h>
#include <AFMotor.h>
// set to 1 for debug output
#define DEBUG 1
// 1. Loc8tor Device parameters
// maximum number of samples to take to determine pitch
// of the piezo sounder used by the Loc8tor to indicate position
#define MAX_PIEZO_SAMPLES 4000
// the maximum average reading on the piezo pin we can expect for
// distance estimation purposes
#define PIEZO_MAX_READING 481
// the minimum average reading on the piezo pin we can expect
// distance estimation purposes
#define PIEZO_MIN_READING 425
// maximum buffer size for keyboard input on the monitor
#define MAX_INPUT_BUFFER 50
// number of readings to take to determine distance once
// the device is in place
#define READING_SAMPLES 10
// The number of steps these particular types of stepper motor has (48)
#define MOTORSTEPS 48
// The z sweep requires about a 3rd of the total rotation (18)
#define Z_MOTOR_STEPS 18
// the rotation speed of the motor in revolutions per minute
#define MOTOR_SPEED 30
// the number of steps to take on each movement
#define MOTOR_INCREMENTS 1
// these refer to the individual port numbers on the shield but
// also act as a handy enumeration for them
#define HORIZONTAL 1
#define VERTICAL 2
// This is the Piezo sounder pin number, the pitch will be used
// to approximate distance
int piezoPin = 5;
// This is the power switch pin
int powerSwitch = 17;
// The search Buttons, there are 4 all together on the Loca8tor
// Just use one for the moment
int searchButton1 = 18;
// 2. Motor related parameters
int sweepsPerSearch = 1;
AF_Stepper motorHorizontal(MOTORSTEPS, HORIZONTAL);
AF_Stepper motorVertical(MOTORSTEPS, VERTICAL);
int mode = MICROSTEP;
// 3. LCD related parameters
// initialize the library with the numbers of the interface pins
// this is modified constructor that I added to the library
int latchPin = 14;
int clockPin = 15;
int dataPin = 16;
LiquidCrystal lcd(latchPin, clockPin, dataPin, 1, 2, 3, 4, 5, 6);
void setup() {
// 1. Loc8tor related initiliation
pinMode(piezoPin, INPUT);
pinMode(searchButton1, OUTPUT);
pinMode(powerSwitch, OUTPUT);
// set both the search and power buttons to LOW
// as soon as possible to stop the device switching on
// automatically when the program first runs
digitalWrite(searchButton1, LOW);
digitalWrite(powerSwitch, LOW);
// 2. Motor initialisation
motorHorizontal.setSpeed(MOTOR_SPEED);
motorVertical.setSpeed(MOTOR_SPEED);
// 3. LCD initilisation
// pinMode will be called in the LiquidCrystal library
// for the latch, clock and data pins
// set up the LCD's number of rows and columns:
lcd.begin(16, 2);
lcdPrint("init...");
delay(1000);
// set up for serial keyboard input
Serial.begin(9600);
Serial.flush();
Serial.println("ready");
/* lcdPrint("about to scan ...");
delay (15000);
doScan(); */
}
void loop() {
if (isKeyboardInput()) {
char readBuffer[MAX_INPUT_BUFFER];
Serial.print ("read: ");
getKeyboardInput(readBuffer, MAX_INPUT_BUFFER-1);
Serial.println (readBuffer);
Serial.flush();
// this section allows commands to be read from the monitor
// to influence the device behaviour
// power the device on or off
if (strcmp(readBuffer, "p") == 0 ){
togglePowerSwitch();
}
// start/stop the scanning function
if (strcmp(readBuffer, "s") == 0) {
doScan();
}
// displat the next 20 readings from the piezo Pin
if (strcmp(readBuffer, "d") == 0) {
for (int i = 0; i < 20; i++) {
Serial.println(analogRead(piezoPin));
}
toggleSearchSwitch();
}
// display the status of the Loc8tor device
if (strcmp(readBuffer, "?") == 0) {
Serial.print("power: ");
Serial.println(isPowerOn() == true? "true" : "false");
Serial.print("scanning: ");
Serial.println(isScanning() == true? "true" : "false");
}
}
char disp[16];
if (!isPowerOn()) {
lcdPrint("no power");
}
else {
float reading = getReading();
if (isScanning()) {
if (reading != 0) {
lcdPrint (dtostrf(reading, 15, 1, disp));
}
else
{
lcdPrint("no signal");
}
}
else
{
lcdPrint("not scanning");
}
}
delay (1000);
}
boolean isKeyboardInput() {
// returns true is there is any characters in the keyboard buffer
return (Serial.available() > 0);
}
void getKeyboardInput(char* readString, int readStringLen){
// take a buffer readString and fill it with characters from the
// keyboard stream up to readStringLen characters
int index=0;
int numChar;
delay (500);
while (numChar = Serial.available()) {
while (numChar-- && (index < readStringLen)) {
readString[index++] = Serial.read();
}
}
// terminate the string
readString[index] = '\0';
}
void printArray (float data[], char* text, int n) {
Serial.print(text);
for (int i = 0; i<n; i++) {
Serial.print(" ");
Serial.print(data[i]);
}
Serial.println("");
}
void printArray (int data[], char* text, int n) {
Serial.print(text);
for (int i = 0; i<n; i++) {
Serial.print(" ");
Serial.print(data[i]);
}
Serial.println("");
}
// report on whether the scanning function is operational
// to do this take a few readings and see if they are approximately
// the same; if they are the same then there is no scanning taking place
boolean isScanning() {
int min = 999;
int max = 0;
// take several readings with a small delay and see if the
// values are varying over time
for (int i=0; i<20; i++) {
int thisReading = analogRead(piezoPin);
if (thisReading > max) max = thisReading;
if (thisReading < min) min = thisReading;
delay (20);
}
//Serial.println(max-min);
return (max - min > 10) ? true : false;
}
// report on whether the power has been activated. If two consecutive readings
// of the piezo device are zero then there is no power.
boolean isPowerOn() {
return ((analogRead(piezoPin) != 0 ) || ( analogRead(piezoPin) != 0 ));
}
void togglePowerSwitch() {
Serial.println("power switch");
digitalWrite(powerSwitch, HIGH);
delay (3000);
digitalWrite(powerSwitch, LOW);
}
void toggleSearchSwitch() {
Serial.println("scan switch");
digitalWrite(searchButton1, HIGH);
delay (3000);
digitalWrite(searchButton1, LOW);
}
// estimate the distance based on the reading from the
// piezo Pin
float getEstimatedDistance(float reading) {
// this is a simple model of deriving distance from a reading.
// This is based on some metrics I gathered in metres
// and modelled using the inverse square law. Multiplying by 100 at the
// end gives the results in cm
// if we are out of the expected ranges then the model breaks down so return 0
if ((reading > PIEZO_MAX_READING) || (reading < PIEZO_MIN_READING)) return 0.0;
return sqrt(28/(PIEZO_MAX_READING - reading) - 0.5) * 100.0f;
}
float logn (float x, float r) {
// use the fact that LOGr(x) = LOG(x)/LOG(r)
return log(x)/log(r);
}
float getReading() {
long averageReading = 0;
// take the average of many readings
for (int i=0; i < MAX_PIEZO_SAMPLES; i++) {
averageReading+=analogRead(piezoPin);
}
return ((float) averageReading/(float) MAX_PIEZO_SAMPLES);
}
// lcd printing helpers
void lcdPrint (char* text, boolean clr, int xPos, int yPos) {
if (clr == true) lcd.clear();
lcd.setCursor(xPos, yPos);
lcd.print(text);
}
void lcdPrint (char* text, boolean clr) {
lcdPrint(text, clr, 0, 0);
}
void lcdPrint (char* text) {
lcdPrint(text, true, 0, 0);
}
int sweep_x_y(int sweepCount) {
float readings[MOTORSTEPS];
int readingCounts[MOTORSTEPS];
// initiliase all variables to 0
for (int i=0; i<MOTORSTEPS; i++) {
readings[i] = 0.0;
readingCounts[i] = 0;
}
// perform sweepCount number of sweeps, taking readings as we go and
// then return to the direction of best signal. Function returns
// the step position it has moved to.
// A sweep is in 4 parts:
// - 180 degrees forward
// - 360 backward
// - 180 degrees forward
// - move as appropriate to strongest signal
// as we move through each point twice, we can take two reading
// and average them
for (int sweeps=0; sweeps<sweepCount; sweeps++) {
// 180 degrees forward
for (int i=MOTORSTEPS/2; i<MOTORSTEPS; i++) {
readings[i] = addReading(i, readings[i], readingCounts[i], getReading());
readingCounts[i]= readingCounts[i]+1;
motorHorizontal.step(MOTOR_INCREMENTS, FORWARD, mode);
//if (DEBUG) logger ("position %d was %s", i, readings[i]);
}
// 360 degrees backwards
/* for (int i=MOTORSTEPS-1; i>0; i--) {
readings[i] = addReading(i, readings[i], readingCounts[i], getReading());
readingCounts[i]= readingCounts[i]+1;
motorHorizontal.step(MOTOR_INCREMENTS, BACKWARD, mode);
// if (DEBUG) logger ("position %d was %s", i, readings[i]);
} */
motorHorizontal.step(MOTORSTEPS, BACKWARD, mode);
// 180 degrees forward to return to the starting point
for (int i=0; i<MOTORSTEPS/2; i++) {
readings[i] = addReading(i, readings[i], readingCounts[i], getReading());
readingCounts[i]= readingCounts[i]+1;
motorHorizontal.step(MOTOR_INCREMENTS, FORWARD, mode);
// if (DEBUG) logger ("position %d was %s", i, readings[i]);
}
}
if (DEBUG) printArray(readings, "sweep_x_y", MOTORSTEPS);
// now check the best 3 signals again and plump for the best out of those
int orderedReadings[MOTORSTEPS];
sortOrder(readings, orderedReadings, MOTORSTEPS);
if (DEBUG) {
Serial.println("checking out...");
Serial.print("pos 1 ");
Serial.print(orderedReadings[0]);
Serial.print (" ");
Serial.println(readings[0]);
Serial.print("pos 2 ");
Serial.print(orderedReadings[1]);
Serial.print (" ");
Serial.println(readings[1]);
Serial.print("pos 3 ");
Serial.print(orderedReadings[2]);
Serial.print (" ");
Serial.println(readings[2]);
}
moveToPosition(HORIZONTAL, MOTORSTEPS/2, orderedReadings[0]);
float reading1 = getAveragedReading(READING_SAMPLES);
displayReadingOnLCD("r1: ", orderedReadings[0], reading1);
if (DEBUG) {
Serial.print("pos 1 was ");
Serial.println(reading1);
}
moveToPosition(HORIZONTAL, orderedReadings[0], orderedReadings[1]);
float reading2 = getAveragedReading(READING_SAMPLES);
displayReadingOnLCD("r2: ", orderedReadings[1], reading2);
if (DEBUG) {
Serial.print("pos 2 was ");
Serial.println(reading2);
}
moveToPosition(HORIZONTAL, orderedReadings[1], orderedReadings[2]);
float reading3 = getAveragedReading(READING_SAMPLES);
displayReadingOnLCD("r3: ", orderedReadings[2], reading3);
if (DEBUG) {
Serial.print("pos 1 was ");
Serial.println(reading3);
}
if ((reading1 < reading2) && (reading1 < reading3)) {
moveToPosition(HORIZONTAL, orderedReadings[2], orderedReadings[0]);
return orderedReadings[0];
}
else {
if ((reading2 < reading1) && (reading2 < reading3))
{
moveToPosition(HORIZONTAL, orderedReadings[2], orderedReadings[1]);
return orderedReadings[1];
}
else {
// no need to move as at this position already
return orderedReadings[2];
}
}
}
int sweep_z(int sweepCount) {
float readings[Z_MOTOR_STEPS];
int readingCounts[Z_MOTOR_STEPS];
// initiliase all variables to 0
for (int i=0; i<Z_MOTOR_STEPS; i++) {
readings[i] = 0.0;
readingCounts[i] = 0;
}
for (int sweeps=0; sweeps<sweepCount; sweeps++) {
// 90 degrees forward
for (int i=Z_MOTOR_STEPS/2; i<Z_MOTOR_STEPS; i++) {
readings[i] = addReading(i, readings[i], readingCounts[i], getReading());
readingCounts[i]= readingCounts[i]+1;
motorVertical.step(MOTOR_INCREMENTS, FORWARD, mode);
}
// 180 degrees backwards
/* for (int i=Z_MOTOR_STEPS-1; i>=0; i--) {
readings[i] = addReading(i, readings[i], readingCounts[i], getReading());
readingCounts[i]= readingCounts[i]+1;
motorVertical.step(MOTOR_INCREMENTS, BACKWARD, mode);
} */
motorVertical.step(Z_MOTOR_STEPS, BACKWARD, mode);
// 90 degrees forward to return to the starting point
for (int i=0; i<Z_MOTOR_STEPS/2; i++) {
readings[i] = addReading(i, readings[i], readingCounts[i], getReading());
readingCounts[i]= readingCounts[i]+1;
motorVertical.step(MOTOR_INCREMENTS, FORWARD, mode);
}
}
if (DEBUG) printArray(readings, "sweep_z", Z_MOTOR_STEPS);
// now check the best 3 signals again and plump for the best out of those
int orderedReadings[Z_MOTOR_STEPS];
sortOrder(readings, orderedReadings, Z_MOTOR_STEPS);
if (DEBUG) {
Serial.println("checking out...");
Serial.print("pos 1 ");
Serial.print(orderedReadings[0]);
Serial.print (" ");
Serial.println(readings[0]);
Serial.print("pos 2 ");
Serial.print(orderedReadings[1]);
Serial.print (" ");
Serial.println(readings[1]);
Serial.print("pos 3 ");
Serial.print(orderedReadings[2]);
Serial.print (" ");
Serial.println(readings[2]);
}
moveToPosition(VERTICAL, Z_MOTOR_STEPS/2, orderedReadings[0]);
float reading1 = getAveragedReading(READING_SAMPLES);
displayReadingOnLCD("r1: ", orderedReadings[0], reading1);
if (DEBUG) {
Serial.print("pos 1 was ");
Serial.println(reading1);
}
moveToPosition(VERTICAL, orderedReadings[0], orderedReadings[1]);
float reading2 = getAveragedReading(READING_SAMPLES);
displayReadingOnLCD("r2: ", orderedReadings[1], reading2);
if (DEBUG) {
Serial.print("pos 2 was ");
Serial.println(reading2);
}
moveToPosition(VERTICAL, orderedReadings[1], orderedReadings[2]);
float reading3 = getAveragedReading(READING_SAMPLES);
displayReadingOnLCD("r3: ", orderedReadings[2], reading3);
if (DEBUG) {
Serial.print("pos 3 was ");
Serial.println(reading3);
}
if ((reading1 < reading2) && (reading1 < reading3)) {
moveToPosition(VERTICAL, orderedReadings[2],orderedReadings[0]);
return orderedReadings[0];
}
else {
if ((reading2 < reading1) && (reading2 < reading3))
{
moveToPosition(VERTICAL, orderedReadings[2],orderedReadings[1]);
return orderedReadings[1];
}
else {
// no need to move as at this position already
return orderedReadings[2];
}
}
}
float getAveragedReading(int readingSamples) {
float total = 0.0;
for (int i=0; i<readingSamples; i++) {
total+=getReading();
}
return total/readingSamples;
}
void moveToPosition(int motor, int current, int pos) {
if (DEBUG) logger ("moveToPosition called current %d, position %d", current, pos);
if (motor == HORIZONTAL) {
motorHorizontal.step(abs(current - pos), current-pos>0?BACKWARD:FORWARD, mode);
}
if (motor == VERTICAL) {
motorVertical.step(abs(current - pos), current-pos>0?BACKWARD:FORWARD, mode);
}
}
float addReading(int i, float averagedReading, int readingCount, float reading) {
displayReadingOnLCD("pos: ", i, reading);
Serial.println(free_memory());
int barLevel = ((int) reading) - 400;
constrain(barLevel, 0, 99);
Serial.print(i);
Serial.print("|");
Serial.print(reading);
Serial.print("|");
for (int i=0; i<barLevel; i++) Serial.print("#");
Serial.println("");
return (((averagedReading) * (float) (readingCount)) + reading) / (readingCount+1);
}
int getBestSignal(float readings[], int count) {
float minimum = 999;
int bestPosition = 0;
for (int i=0; i< count; i++) {
Serial.print(readings[i]);
Serial.print(" ");
if (readings[i] < minimum) {
minimum = readings[i];
bestPosition = i;
}
}
if (DEBUG) logger("best Position was %d", bestPosition);
return bestPosition;
}
void logger (const char* logstring, int arg1, int arg2) {
char buffer[50];
sprintf(buffer, logstring, arg1, arg2);
Serial.println(buffer);
Serial.flush();
}
void logger (const char* logstring, int arg1) {
char buffer[50];
sprintf(buffer, logstring, arg1);
Serial.println(buffer);
Serial.flush();
}
void doScan() {
if (!isPowerOn()) togglePowerSwitch();
if (!isScanning()) toggleSearchSwitch();
// wait for scanning to start
delay(8000);
// stop it waggling about when the x_y motor is moving
// lockMotor(VERTICAL);
int x_y_pos = sweep_x_y(sweepsPerSearch);
int z_pos = sweep_z(sweepsPerSearch);
// we are now pointing at the tag; take a good reading to
// establish distance
float distance = getAveragedReading(READING_SAMPLES);
// now report the results
char line1[16];
char line2[16];
char floatNum[8];
sprintf(line1, "H: %d, V: %d", x_y_pos, z_pos);
Serial.print("reading: ");
Serial.print(distance);
Serial.print(" estimated distance: ");
Serial.println(getEstimatedDistance(distance));
line2[0] = '\0';
strcat(line2, "dist: ");
strcat(line2, dtostrf(getEstimatedDistance(distance), 5, 2, floatNum));
strcat(line2, "cm");
Serial.println(strlen(line2));
Serial.println(strlen(floatNum));
lcdPrint (line1, true, 0, 0);
lcdPrint (line2, false, 0, 1);
if (DEBUG) logger ("back to orignal positions relative x/y = %d, z = %d", (int) MOTORSTEPS/2 - x_y_pos, (int) Z_MOTOR_STEPS/2 - z_pos);
moveToPosition(VERTICAL, z_pos, Z_MOTOR_STEPS/2);
moveToPosition(HORIZONTAL, x_y_pos, MOTORSTEPS/2);
delay (5000);
if (isScanning()) toggleSearchSwitch();
if (isPowerOn()) togglePowerSwitch();
}
// sort an array of n integers and return the array indexes of the sorted
// array. Sort is largest to smallest
void sortOrder (float arr[], int retArray[], int n) {
// initialise an array of int 0, 1, 2 ... n
for (int k=0; k<n;k++) retArray[k] = k;
if (DEBUG) {
printArray(arr, "array to sort", n);
}
float tmpNum; //used to swap values of two array entries
int tmpInt; //used to swap the associated indexes for those values
for (int i = 0; i < n; i++) {
for (int j = 0; j < n - 1; j++) {
if (arr[j] > arr[j + 1]) {
tmpNum = arr[j];
arr[j] = arr[j + 1];
arr[j + 1] = tmpNum;
tmpInt = retArray[j];
retArray[j] = retArray[j + 1];
retArray[j + 1] = tmpInt;
}
}
}
if (DEBUG) {
printArray(arr, "sorted", n);
printArray(retArray, "retArray", n);
}
}
void lockMotor(int motor) {
// simulate locking the motor by moving it back and forward
// numbers are arbitrary as the moveToPosition call is relevant
moveToPosition(motor, 1, 0);
moveToPosition(motor, 0, 1);
}
void displayReadingOnLCD(char* text, int i, float reading){
char disp[16];
lcdPrint(text, true, 0, 0);
lcdPrint(itoa(i,disp, 10), false, strlen(text)+1, 0 );
lcdPrint (dtostrf(reading, 15, 1, disp), false, 0, 1);
free (disp);
}
extern int __bss_end;
extern int *__brkval;
int free_memory(){
int free_memory;
if((int)__brkval == 0)
free_memory = ((int)&free_memory) - ((int)&__bss_end);
else
free_memory = ((int)&free_memory) - ((int)__brkval);
return free_memory;
}