// include the distance sensor library
#include "Ultrasonic.h"
// Define the pins
const int ledPins[4] = {11, 10, 6, 5}; // LED pins
const int speakerPin = 9; // speaker pin
const int trigPin = 12; // "trigger" pin for the distance sensor
const int echoPin = 13; // "echo" pin for the distance sensor
// create an instance of the distance sensor
Ultrasonic ultrasonic( trigPin, echoPin );
// array to store the last 5 distance readings
// it is used to smooth out any sudden jumps
int histValues[5] = {201, 201, 201, 201, 201};
// next position in the value array above
int indValue = 0;
// Define some operational timers
unsigned long sensorNext = 0; // timer to schedule sensor polling
unsigned long servoNext = 0; // timer to schedule servo movement
// Initial servo positions (0 is 90 degrees)
int servoPos = 0;
// Initial servo direction (1 = forward from 90 degrees, -1 = backward)
int servoDir = 1;
// function to set a particular servo on pololu (0-7) to an angle
// the angle from 0 to 180 is loosely transformed into absolute servo position
void setServoPos (unsigned int servoNum, unsigned int angle) {
// command array (6 bytes)
unsigned char command[6];
// remap the angle to adjusted pololu operational range
// even though it should be 500-5500, it works better with 500-5000
unsigned int tempAngle = map (angle, 0, 180, 500, 5000);
// calculate the "high" position byte
// by zeroing out the 7 lest significant bits
// and shifting the other bits right by 7 bits
unsigned int posHigh = (tempAngle&0x1F80)>>7;
// calculate the "low" position byte
// by zeroing out everything but the 7 least significant bits
unsigned int posLow = tempAngle&0x7F;
command[0] = 0x80; // start byte
command[1] = 0x01; // device ID
command[2] = 0x04; // set absolute position command
command[3] = servoNum; // servo number
command[4] = posHigh; // position (high byte)
command[5] = posLow; // position (low byte)
// write the command to pololu via serial port
Serial.write(command,6);
// wait for pololu to process
delay(10);
}
// function to set the servo speed based on the current distance
// expects preset "servo position" (1-4) as input to map to speed
// speed is capped at 70 to avoid wild swings and epic destruction
void setServoSpd (unsigned int servoNum, unsigned int servoPos) {
// command array (5 bytes)
unsigned char command[5];
// remap the position value (1-4) to speed (1-70)
unsigned int tempSpeed = map (servoPos, 1, 4, 1, 70);
command[0] = 0x80; // start byte
command[1] = 0x01; // device ID
command[2] = 0x01; // set speed command
command[3] = servoNum; // servo number
command[4] = tempSpeed; // speed
// write the command to pololu via serial port
Serial.write(command,5);
// wait for pololu to process
delay(10);
}
// initialize the installation
void setup()
{
// mute the speaker
noTone(speakerPin);
// initialize serial port to pololu (38400 baud)
Serial.begin(38400);
// let pololu sync
delay(200);
// set servo speed to max (position 4)
setServoSpd(0,4);
delay(500);
// move servo 0 to angle 90
setServoPos(0,90);
// schedule the sensor to be polled in 200 ms
sensorNext = millis() + 200;
}
// main program
void loop()
{
int i;
// check if the sensing routing is scheduled to run based on the timer
// this routine fills out the historical distance valus array, adjusts
// the current speed and position of the servo, and lights the LEDs
if (sensorNext <= millis()) {
// read the distance from sensor
int distanceValue = ultrasonic.Ranging(CM);
// if the distance is over 200cm (2m), we need to turn off
if (distanceValue >= 200)
distanceValue = 201; // 201 here is "infinity"
// add the currently read distance to the next slot in historical values array
histValues[indValue] = distanceValue;
// increment array indes
indValue++;
// if we already have 5 values, wrap around to 0
if (indValue == 5)
indValue = 0;
// figure out the minimum distance from the historical values array
// the initial setting is 201 (infinity)
int valMin = 201;
// step through the array and find the lower value
// we take the lowest value within past 5 seconds as the distance
for (i = 0; i < 5; i++) {
if (histValues[i] < valMin)
valMin = histValues[i];
}
// need to make notifications only if one of the past measure distances is under 201
if (valMin != 201) {
// remap the read distance (0-200) to speaker tone (the closer, the higher)
distanceValue = map(valMin, 0, 200, 3700, 10);
// set the speaker to output the tone
tone(speakerPin, distanceValue);
// remap the read distance (0-200) to a preset postion for the swing (4-1)
// the closer the object is, the higher is the speed setting and angle
servoPos = map (valMin, 0, 200, 4, 1);
// set the LEDs to variable brightness based on the distance
for (i = 0; i < 4; i++)
// the closer the object is, the brighter the LEDs
analogWrite(ledPins[i], map (valMin, 0, 200, 255, 1));
// if servo movement is not scheduled, schedule it in 10ms
if (servoNext == 0)
servoNext = millis() + 10;
// if all the historical distances are 201 (infinity), we should stop activity
} else {
// move the swing to neutral position (90 degrees)
servoPos = 0;
// mute the speaker
noTone(speakerPin);
// turn off all LEDs
for (i = 0; i < 4; i++)
analogWrite(ledPins[i],0);
}
// schedule the next sensor poll in a second
sensorNext = millis() + 1000;
}
// check if it's time to move the swing
if (servoNext <= millis()) {
// if the requested position is 0, move to neutral (90 degrees)
if (servoPos == 0) {
setServoPos(0, 90);
// reset the direction to forward
servoDir = 1;
// otherwise, start swinging
} else {
// set the speed based on position (1-4, 4 being the fastest)
setServoSpd(0,servoPos);
// set the servo position to move the swing
// each position(1-4) increases the movement angle from 90 degrees by 5 degrees
// so, with position 1, the angles for the swing are 85-95
// with position 4, the angles for the swing are 70-110
// this should be adjusted with care to avoid damage to the swing
setServoPos(0,90+5*servoDir*servoPos);
// after moving the swing, reverse the future direction to backward
servoDir = -1*servoDir;
// schedule the next servo movement
// 0.7 second delay is allowed per swing at speed = 4
// 3.5 second delay is allowed per swing at speed = 1
// adjust the delay with care to avoid damage
servoNext = millis()+700*(5-servoPos);
}
}
}