Haptic Wrist Rangefinder Pt. 4 – Programming
One of our main displays for the DIY Tech section is the Haptic Wrist Rangefinder, modelled off of Steve Hoefer‘s MAKE:Project called Tacit. The rangefinder uses ultrasonic sensors to determine its distance from objects, and then translates that distance into haptic (touch-based) feedback, with servomotors that provide light pressure on your wrist corresponding to the object’s proximity. For more information on how the technology actually works, check out the Haptic Wrist Rangefinder Pt 1!
Once we had all the components attached onto the glove, we needed to program the Arduino so that the rangefinder would actually work! Programming this took way longer than expected, as we ran into numerous glitches and problems, even using the Tacit program as a starting point, mainly because of differences in the way our sensors worked.
Our ultra-cheap, yet fairly accurate HY-SRF05 ultrasonic sensors have 5 pins, as mentioned in the “Bits & Pieces” post, while the Parallax PING))) ultrasonic sensors have only 3. This meant that the Tacit project code needed to be modified so that it would write to one pin, and read from another, rather than sending to and receiving from the same pin. Next, we realized that our sensors return a value of 0 when the distance is too far away to be measured, while the PING))) sensors must return a high value. This led to problems where, with the Tacit project code, our rangefinder defaulted to the highest pressure when objects moved too far away, leading to jolting and erroneous servomotor positioning. We had to add a supplemental if statement to convert all the 0-distance outputs to high values to combat that. Finally, we learned that some of the lights, ventilation fans, or other equipment in the ceiling of the exhibit space was emitting ultrasonic frequencies that were interfering with the sensors, causing them to drive the servomotors back and forth non-stop. We got around that by modifying the historical averaging function within the Tacit program code to smooth out the periodic spikes caused by whatever was in the ceiling.
Below is a copy of our modified code:
// Tacit, Wrist mounted tactile feedback for the blind.
// By Steve Hoefer at Grathio Labs (http://grathio.com)
// Version 12.02.04
//
// Copyright (c) 2012 Steve Hoefer
// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and
// associated documentation files (the “Software”), to deal in the Software without restriction,
// including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so,
// subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all copies or substantial
// portions of the Software.
//
// THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT
// LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN
// NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
// WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
// SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
//
//
// Written for Arduino authoring environment version 0022 and a Arduino Mini Pro 5V but should work on any Arduino/Arduino compatible that provides 5 volts.
//
// This version supports the following hardware:
// Parallax PING))) ultrasonic sensors for range finding
// Connect the GND pin to ground, +5V pin to +5V, and SIG to pin 2 or 3.
// Just about any small hobby servo. (Specifically Turnigy TG9)
// Connect the ground to ground, +V to RAW and the signal to pins 7 or 8.
//
//
// Version history:
// 11.08.07 – original
// 11.08.14 – Added code so servos apply constant pressure even when readings are not changing.
// 11.10.11 – Added pause after each sensor reading to fix reported issue with left sensor occasionally giving garbage responses.
// 12.02.04 – Changed license from CC BY-NC-SA to the MIT license.#include <Servo.h>
const int MaxSensors = 2; // The number of sensor/servo pairs.
const int ServoPins[MaxSensors] = {8, 7}; // The pins they’re on
const int TriggerPins[MaxSensors] = {2, 3}; // The pins they’re on
const int EchoPins[MaxSensors] = {4, 5};
const int ReadingsPerSensor = 5; // The number of historic readings to consider when determining position.
const int TimePerDegree = 9; // ms per degree rotation on the servo to prevent servo motor electrical noise from interfering with the ultrasonic sensor readings
const int MinimumTurnDistance = 3; // Minimum number of degrees that the servo will turn. Keeps the servos from being too twitchy.// Variables
Servo ServoList[MaxSensors]; // Array of servo objects for manipulating easily.
int sensorReadings[MaxSensors][ReadingsPerSensor]; // Hold past readings for each sensor.
int calculatedSenorReadings[MaxSensors]; // The calculated distance for each sensor.
int latestReading = 0; // Current position in the array for the most recent reading.
int servoLocations[MaxSensors]; // The current position of each sensor.
int SenorClose = 100; // Closest value we detect with the PING sensor. (Soundwave travel time in milliseconds.)
int SensorFar = 2000; // Furthest distance we register on the PING sensor. (Soundwave travel time in milliseconds.)
int ServoClose[MaxSensors] = {10, 160}; // Angle the servo turns to when something is closest.
int ServoFar[MaxSensors] = {90,110}; // Angle the servo turns to when something is at its furthest.void setup() {
Serial.begin(115200); // Uncomment the Serial.foo lines for testing.
Serial.println(“Begin…”);// Initialize the servo location and move them through a full range of motion so we know they work.
for (int i = 0; i < MaxSensors; i++){
ServoList[i].attach(ServoPins[i]);
delay(10);
ServoList[i].write(ServoClose[i]);
delay(500);
ServoList[i].write(ServoFar[i]);
delay(500);
ServoList[i].detach();
}
delay(100);}
void loop(){
int i, j, oldLocation;
unsigned long delayTime;// Loop through each range sensor
for (i = 0; i < MaxSensors; i++){
// Get the current sensor’s range.
sensorReadings[i][latestReading] = getDistance(i);
// Figure out an averaged/smoothed readings based on this and past data.
calculatedSenorReadings[i] = calculateNewDistace(i);// Set the servo to the correct angle.
oldLocation = servoLocations[i];
servoLocations[i] = map(calculatedSenorReadings[i], 0, 100, ServoClose[i], ServoFar[i]);if (latestReading >= ReadingsPerSensor-1){ // Don’t do anything until we have enough data to trend.
if (abs(servoLocations[i]-oldLocation) >= MinimumTurnDistance){ // Only try to turn it if we have somewhere to go.
ServoList[i].attach(ServoPins[i]);
delay(10);
ServoList[i].write(servoLocations[i]);
delayTime = (TimePerDegree * (abs(servoLocations[i]-oldLocation))+30); // Set a delay for the next reading so motor noise doesn’t interfere with senor readings.
if (abs(delayTime)>500){ // If it can’t do it in this amount of time // It’s based on how far it has to turn to keep the delay to a minimum, response time at a maximum.
delayTime=500; // we’ll get it next time. Keep it responsive.
}
delay(delayTime);
ServoList[i].detach();
} else { // Otherwise if the reading hasn’t changed enough write the old value to
ServoList[i].attach(ServoPins[i]); // the servo so that it will hold in place if it’s applying pressure.
delay(10);
ServoList[i].write(oldLocation);
delay(50);
ServoList[i].detach();
servoLocations[i]=oldLocation;
}
}
delay(30); // Added to fix left sensor misbehavior reported by Rob.
}latestReading++; // Increment the reading counter so we know where we’re at.
if (latestReading >= ReadingsPerSensor){ // Make sure we don’t record more readings than we have space to hold.
latestReading = ReadingsPerSensor-1;
// Pop the oldest reading off the list.
for (i = 0; i < MaxSensors; i++){
for (j=0; j < ReadingsPerSensor-1; j++){
sensorReadings[i][j] = sensorReadings[i][j+1];
}
}
}
}// function: calculateNewDistace(sensorNumber: Which sensor’s data to process): Calculated distance in 0-100 range.
// Apply some averaging and smoothing to the recorded distance readings
// to take care of noisy data.
int calculateNewDistace(int sensorNumber){
int output = SensorFar; // Default value is the furthest distance.float weightingFactor = 0.1; // How fast the reading’s importance tapers off in time. (1= no taper, 0 = divide by zero error.)
float flickerFactor = 30; // When the change is greater than this, ignore it unless its two in a row. (It’s probably noise.)if (latestReading >= ReadingsPerSensor-1) { // Only do this if we have a full set of readings to sample.
int total = 0; // Average them with a weighting.
float currentWeight = 1; // New readings count more than older readings.
float percentagePossible = 0;
boolean flickered = false;
for (int i=ReadingsPerSensor-1; i >=0 ;i–){ // Check for flicker (This reduces jitter with something right on the threshold.)
flickered = false;
if (i==ReadingsPerSensor-1){
if ((abs(sensorReadings[sensorNumber][i])-abs(sensorReadings[sensorNumber][i-1]) > flickerFactor) &&
(abs(sensorReadings[sensorNumber][i-1])-abs(sensorReadings[sensorNumber][i-2]) > flickerFactor)){
flickered = true;
}
}
if (flickered==false){
total += (sensorReadings[sensorNumber][i] * currentWeight);
percentagePossible += currentWeight;
currentWeight *= weightingFactor;
}
}
output = total / percentagePossible;
}
return output;
}
// function: getDistance
// Take a sensor number (not pin number) and returns an int in the 0-100 range
// 0 = closest, 100= furthest. (It’s a percentage of the distance that the software
//
// Note: Function is designed to be generic so that it can be swapped out for
// different kinds of ranging sensors.
// This version of the function is made for Parallax PING))) sensors
// For more info see http://arduino.cc/en/Tutorial/Ping
// and http://www.parallax.com/tabid/768/ProductID/92/Default.aspx
int getDistance(int sensorNumber){
long duration; // How long it takes a sonic pulse to reflect back.
int out; // The value we send back from the function// Initialize the sensor and tell it to send out a ping.
pinMode(TriggerPins[sensorNumber], OUTPUT);
digitalWrite(TriggerPins[sensorNumber], LOW);
delayMicroseconds(2);
digitalWrite(TriggerPins[sensorNumber], HIGH);
delayMicroseconds(5);
digitalWrite(TriggerPins[sensorNumber], LOW);// Read the time in milliseconds until the value comes back.
pinMode(EchoPins[sensorNumber], INPUT);
duration = pulseIn(EchoPins[sensorNumber], HIGH);// Trim the data into minimums and maximums and map it to the 0-100 output range.
if (duration < 100){
duration = 2000;
}
Serial.print(duration);
duration = constrain(duration, SenorClose, SensorFar);
out = map(duration, SenorClose, SensorFar, 0, 100);
return out;
}