Yep! I can see some interesting possibilities with this laser range finder. Iām going to get one after I have W.A.L.T.E.R. doing basic movement using the three IR sensors. Iāll replace the front PING sensor when I get more money (soon!).
I have the front pan/tilt working now, and have all the limits set in a Python script. Iāve also expanded my Arduino two channel sound detection code to four channels for front/left, front/right, back/left, and back/right detection directions. Iāve broken the code into three routines and am storing the sound sample data in a struct. Iām using an array of struct for data storage and am using loops to process everything. Itās very fast! My Arduino navigation code (including the sound detection code) is now 478 lines and 38% of the Arduinoās capacity. Itās all running on my BotBoarduino now, and I am awaiting aquisition of four new electret mics to complete the sound detection circuit - this takes four of the BotBoarduinoās analog inputs, still leaving A4 and A5 for I2C.
I have a two channel detection setup running on my true blue Arduino now, and it works great. I need 3 analog inputs for the IR sensors and 4 analog inputs for the sound detection circuit, so I am going to have to move the sound detection stuff to another board. Iām also looking at the possibility of using an Arduino Nano with an expansion board I found on RobotShop yesterday, because it has 8 analog inputs. Even the Nano may not give me enough, but I need to look at its pinout to see where the I2C pins are located. Hopefully I2C wonāt conflict with the analog inputs.
Iām also starting to play with the Wire library to set my BotBoarduino up as an I2C slave. Iāll connect it to Pod (Raspberry Pi) for testing. Itās quite possible that Pod may end up mounted on W.A.L.T.E.R. as soon as I work out the power connection setup and source I want to use. I got a BEC from Adafruit that takes in 6V to 20V and puts out a smooth 5V, so I just need to wire up a power connector for Pod. I still have to test I2C with Stargazer (BBBlack), and get some connectors soldered to Intrepid (PandaBoard ES) so I can make more use of it for stuff.
Iāve also got initialization code in my navigation sketch for W.A.L.T.E.R. that sets up all the I2C and other sensors I have. That includes my 10DOF IMU, TCS34725 RGB color sensor, TMP006 heat sensor, and DS1307 real time clock. Pod has the camera board too. I got all these sensors from Adafruit. I also have their I2C 16 channel servo breakout board.
So, as you can see, Iāve been quite busy!
I am loving working with the BotBoarduino! Iām planning on wiring up the Sabertooth 2x5 and SSC-32 to it today. Iāll probably go with PWM control for the motor controller, for my first tests with W.A.L.T.E.R. moving around. I have a webcam setup and pointed at my work area now. It runs on Stargazer at present, with my Logitech Pro 9000 webcam.
I need a good solution for analog input expansion, but the best I have found so far are 4 channel breakouts that require I2C. I may have to look at using SPI for some stuff. Below is my current code for W.A.L.T.E.R. - FAR from complete, and the sound detection code remains untested until I can connect a full four channel mic circuit.
/*
W.A.L.T.E.R. 2.0, main navigation and reactive behaviors
Credit is given, where applicable, for code I did not write.
Copyright (C) 2013 Dale Weber <[email protected]>.
*/
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BMP180_Unified.h>
#include <Adafruit_LSM303DLHC_Unified.h>
#include <Adafruit_L3GD20.h>
#include <Adafruit_TCS34725.h>
#include <Adafruit_TMP006.h>
#include <RTClib.h>
#include "Walter_Navigation.h"
/*
Initialize our sensors
We have:
A BMP180 temperature and pressure sensor
An L3GD20 Gyro
An LSM303 3-Axis accelerometer / magnetometer (compass)
A TCS34725 Color sensor
A TMP006 Heat sensor
A DS1307 Real time clock
GP2D12 IR Ranging sensors (3)
*/
/*
These are all on a single small board from Adafruit
http://www.adafruit.com/products/1604
*/
Adafruit_BMP180_Unified temperature = Adafruit_BMP180_Unified(10001);
Adafruit_L3GD20 gyro;
Adafruit_LSM303_Accel_Unified accelerometer = Adafruit_LSM303_Accel_Unified(10002);
Adafruit_LSM303_Mag_Unified magnetometer = Adafruit_LSM303_Mag_Unified(10003);
/*
These are also from Adafruit, in order.
http://www.adafruit.com/products/1334 (TCS34725)
http://www.adafruit.com/products/1296 (TMP006)
http://www.adafruit.com/products/264 (DS1307)
*/
Adafruit_TCS34725 color = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_4X);
Adafruit_TMP006 heat;
RTC_DS1307 ds1307;
/*
Initialize global variables
*/
// Storage for sound detection sample data
Sample samples[SAMPLE_MAX];
// These are where the sensor readings are stored.
int ping[MAX_PING];
float gp2d12[MAX_GP2D12];
/*
Function that reads a value from GP2D12 infrared distance sensor and returns a
value in centimeters.
This sensor should be used with a refresh rate of 36ms or greater.
Javier Valencia 2008
float read_gp2d12(byte pin)
It can return -1 if something gone wrong.
TODO: Make several readings over a time period, and average them
for the final reading.
*/
float read_gp2d12 (byte pin) {
int tmp;
tmp = analogRead(pin);
if (tmp < 3)
return -1; // Invalid value
else
return (6787.0 /((float)tmp - 3.0)) - 4.0; // Distance in cm
}
/*
Convert the PING pulse width in ms to inches
*/
long microsecondsToInches(long microseconds) {
// According to Parallax's datasheet for the PING))), there are
// 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
// second). This gives the distance travelled by the ping, outbound
// and return, so we divide by 2 to get the distance of the obstacle.
// See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
return microseconds / 74 / 2;
}
/*
Convert the pulse width in ms to a distance in cm
*/
long microsecondsToCentimeters(long microseconds) {
// The speed of sound is 340 m/s or 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance travelled.
return microseconds / 29 / 2;
}
/* Ping))) Sensor
This sketch reads a PING))) ultrasonic rangefinder and returns the
distance to the closest object in range. To do this, it sends a pulse
to the sensor to initiate a reading, then listens for a pulse
to return. The length of the returning pulse is proportional to
the distance of the object from the sensor.
The circuit:
* +V connection of the PING))) attached to +5V
* GND connection of the PING))) attached to ground
* SIG connection of the PING))) attached to digital pin 7
http://www.arduino.cc/en/Tutorial/Ping
Created 3 Nov 2008
by David A. Mellis
Modified 30-Aug-2011
by Tom Igoe
Modified 09-Aug-2013
by Dale Weber
This example code is in the public domain.
*/
// Set units = true for inches and false for cm
int readPing (byte pingPin, boolean units) {
// Establish variables for duration of the ping,
// and the distance result in inches and centimeters:
long duration;
int result;
// The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(5);
digitalWrite(pingPin, LOW);
// The same pin is used to read the signal from the PING))): a HIGH
// pulse whose duration is the time (in microseconds) from the sending
// of the ping to the reception of its echo off of an object.
pinMode(pingPin, INPUT);
duration = pulseIn(pingPin, HIGH);
// Convert the time into a distance
if (units) {
// Return result in inches.
result = microsecondsToInches(duration);
} else {
// Return result in cm
result = microsecondsToCentimeters(duration);
}
delay(100);
return result;
}
/*
Pulses a digital pin for duration ms
*/
void pulseDigital(int pin, int duration) {
digitalWrite(pin, HIGH); // Turn the ON by making the voltage HIGH (5V)
delay(duration); // Wait for duration ms
digitalWrite(pin, LOW); // Turn the pin OFF by making the voltage LOW (0V)
delay(duration); // Wait for duration ms
}
bool displaySample (byte channel, Sample *sample) {
String st;
bool error = false;
switch (channel) {
case FRONT_LEFT_SIDE:
st = "Front Left";
break;
case FRONT_RIGHT_SIDE:
st = "Front Right";
break;
case BACK_LEFT_SIDE:
st = "Back Left";
break;
case BACK_RIGHT_SIDE:
st = "Back Right";
break;
default:
st = "Invalid";
error = true;
break;
}
if (! error) {
Serial.print(st);
Serial.print(" Sample = ");
Serial.print(sample->value);
Serial.print(", Voltage = ");
Serial.print(sample->volts);
Serial.println(" volts");
}
return error;
}
/*
Get a sound sample from the given channel
*/
Sample soundSampleOf (byte channel) {
Sample sample;
sample.value = analogRead(channel);
sample.volts = (sample.value * MAX_VOLTS) / MAX_STEPS;
// Initialize the rest of the sample
sample.signalMin = 1024;
sample.signalMax = 0;
sample.signalMinVolts = 0.0;
sample.signalMaxVolts = 0.0;
sample.peakToPeakVolts = 0.0;
return sample;
}
/*
Get samples of sound from four microphones
*/
void getSoundSamples (Sample samples[SAMPLE_MAX]) {
byte channel;
Sample ts;
// Start of sample window
unsigned long startMillis= millis();
while ((millis() - startMillis) < sampleWindow) {
/*
Get and process raw samples from each microphone
*/
for (channel = 0; channel < CHANNEL_MAX; channel++) {
samples[channel] = soundSampleOf(channel);
ts = samples[channel];
if (ts.value < 1024) {
// Toss out spurious readings
if (ts.value > ts.signalMax) {
// Save just the max levels
ts.signalMax = ts.value;
ts.signalMaxVolts = ts.volts;
} else if (ts.value < ts.signalMin) {
// Save just the min levels
ts.signalMin = ts.value;
ts.signalMinVolts = ts.volts;
}
samples[channel] = ts;
}
}
} // End of sample collection loop
// Calculate the peak to peak voltages
for (channel = 0; channel < CHANNEL_MAX; channel++) {
ts = samples[channel];
ts.peakToPeakVolts = abs(ts.signalMaxVolts - ts.signalMinVolts);
samples[channel] = ts;
}
}
/*
Try to detect the loudest sound from one of four directions
*/
byte detectSound (void) {
unsigned int sampleValue;
Sample *ts;
/*
Variables for sound detection
*/
double detectionFrontVolts = 0.0, detectionBackVolts = 0.0;
byte detectionFront = 0;
byte detectionBack = 0;
byte detectionResult = 0;
int displayPeakFrontLeft, displayPeakFrontRight;
int displayPeakBackLeft, displayPeakBackRight;
/*
Code starts here
*/
/*
Turn all the sound detection LEDs off
*/
digitalWrite(FRONT_LEFT_LED, LOW);
digitalWrite(FRONT_RIGHT_LED, LOW);
digitalWrite(BACK_LEFT_LED, LOW);
digitalWrite(BACK_RIGHT_LED, LOW);
getSoundSamples(samples);
/*
Calculate the FRONT detection value
*/
detectionFrontVolts = abs(samples[FRONT_LEFT_SIDE].peakToPeakVolts - samples[FRONT_RIGHT_SIDE].peakToPeakVolts);
Serial.print("Front Detection value = ");
Serial.println(detectionFrontVolts);
/*
Calculate the BACK detection value
*/
detectionBackVolts = abs(samples[BACK_LEFT_SIDE].peakToPeakVolts - samples[BACK_RIGHT_SIDE].peakToPeakVolts);
Serial.print("Back Detection value = ");
Serial.println(detectionBackVolts);
/*
Get our final detection result
*/
if ((detectionFrontVolts > detectionBackVolts) && (detectionFrontVolts > DETECTION_THRESHOLD)) {
// Check for sound detection
if (samples[FRONT_LEFT_SIDE].peakToPeakVolts > samples[FRONT_RIGHT_SIDE].peakToPeakVolts) {
digitalWrite(FRONT_LEFT_LED, HIGH);
detectionFront = FRONT_LEFT_SIDE;
} else if (samples[FRONT_RIGHT_SIDE].peakToPeakVolts > samples[FRONT_LEFT_SIDE].peakToPeakVolts) {
digitalWrite(FRONT_RIGHT_LED, HIGH);
detectionFront = FRONT_RIGHT_SIDE;
} else {
detectionFront = NO_SOUND_DETECTION;
}
detectionResult = detectionFront;
} else if ((detectionBackVolts > detectionFrontVolts) && (detectionBackVolts > DETECTION_THRESHOLD)) {
// Check for sound detection
if (samples[BACK_LEFT_SIDE].peakToPeakVolts > samples[BACK_RIGHT_SIDE].peakToPeakVolts) {
digitalWrite(BACK_LEFT_LED, HIGH);
detectionBack = BACK_LEFT_SIDE;
} else if (samples[BACK_RIGHT_SIDE].peakToPeakVolts > samples[BACK_LEFT_SIDE].peakToPeakVolts) {
digitalWrite(BACK_RIGHT_LED, HIGH);
detectionBack = BACK_RIGHT_SIDE;
} else {
detectionBack = NO_SOUND_DETECTION;
}
detectionResult = detectionBack;
}
return detectionResult;
}
void setup() {
// Start up the Wire library as a slave device at address 0xE0
Wire.begin(NAV_I2C_ADDRESS);
// Register event handler
Wire.onRequest(wireRequestEvent);
Wire.onReceive(wireReceiveData);
// Initialize the hardware serial port
Serial.begin(115200);
// Initialize the LED pin as an output.
pinMode(HEARTBEAT_LED, OUTPUT);
// Set the LED pins to be outputs
pinMode(COLOR_SENSOR_LED, OUTPUT);
pinMode(FRONT_LEFT_LED, OUTPUT);
pinMode(FRONT_RIGHT_LED, OUTPUT);
pinMode(BACK_LEFT_LED, OUTPUT);
pinMode(BACK_RIGHT_LED, OUTPUT);
// Test the LEDs
digitalWrite(FRONT_LEFT_LED, HIGH);
digitalWrite(FRONT_RIGHT_LED, HIGH);
digitalWrite(BACK_LEFT_LED, HIGH);
digitalWrite(BACK_RIGHT_LED, HIGH);
delay(1000);
digitalWrite(FRONT_LEFT_LED, LOW);
digitalWrite(FRONT_RIGHT_LED, LOW);
digitalWrite(BACK_LEFT_LED, LOW);
digitalWrite(BACK_RIGHT_LED, LOW);
digitalWrite(COLOR_SENSOR_LED, LOW);
}
/*
Called when a request from an I2C (Wire) Master comes in
*/
void wireRequestEvent (void) {
}
// Called when the I2C (Wire) Slave receives data from an I2C (Wire) Master
void wireReceiveData (int nrBytesRead) {
}
// The loop routine runs forever:
void loop() {
byte directionOfSound = 0;
int analogPin = 0;
int digitalPin = 0;
// Pulse the heartbeat LED
pulseDigital(HEARTBEAT_LED, 500);
// Get distance readings from the GP2D12 Analog IR sensors and store the readings
for (analogPin = 0; analogPin < MAX_GP2D12; analogPin++) {
gp2d12[analogPin] = read_gp2d12(analogPin);
}
// Display the GP2D12 sensor readings (cm)
for (analogPin = 0; analogPin < MAX_GP2D12; analogPin++) {
Serial.print("gp2d12 #");
Serial.print(analogPin + 1);
Serial.print(" range = ");
Serial.print(gp2d12[analogPin]);
Serial.println(" cm");
}
Serial.println("");
/*
// Get distance readings from PING Ultrasonic sensors in cm and store them
for (digitalPin = 0; digitalPin < MAX_PING; digitalPin++) {
ping[digitalPin] = readPing(digitalPin + DIGITAL_PIN_BASE, false);
}
// Display PING sensor readings (cm)
for (digitalPin = 0; digitalPin < MAX_PING; digitalPin++) {
Serial.print("Ping #");
Serial.print(digitalPin + 1);
Serial.print(" range = ");
Serial.print(ping[digitalPin]);
Serial.println(" cm");
}
Serial.println("");
*/
// Do sound detection
directionOfSound = detectSound();
if (directionOfSound == NO_SOUND_DETECTION) {
Serial.println("No sound detection");
} else {
Serial.print("Sound detection from the ");
switch (directionOfSound) {
case FRONT_LEFT_SIDE:
Serial.print("FRONT LEFT");
break;
case FRONT_RIGHT_SIDE:
Serial.print("FRONT RIGHT");
break;
case BACK_LEFT_SIDE:
Serial.print("BACK LEFT");
break;
case BACK_RIGHT_SIDE:
Serial.print("BACK RIGHT");
break;
default:
Serial.print("an INVALID");
break;
}
Serial.println(" side");
}
Serial.println();
}
8-Dale