The goal of this project was to create a device that detects ambient presence. A sonar sensor is to be placed on a person’s desk. When the person is present and working at their desk, the partner’s device has a servo that moves an arm to wave hello. The signals are relayed using the MQTT Bridge Python program. The speed of this hand wave is controlled by how close the person is to the sensor, with farther away making the motion slower and closer towards making the motion faster. The delay and motion of the arm was tweaked to not be as fidgety and not as much of a distraction to the user. The below video shows the mechanical and signal processing of the system.
#include <Servo.h> //pin setup const int SERVO_PIN = 5; const int TRIG_PIN = 12; const int ECHO_PIN = 13; //variables for sonar const int MAX_DIST = 450; const int SOUND_SPEED = 34000; const long ECHO_TIME = (2*MAX_DIST*1000000)/SOUND_SPEED; //variables for sonar int rightAngle = 0; int leftAngle = 110; int angleInterval = 10; int servoDelay = 0; int servoAngle; int recievedDelay; Servo myServo; //static variable for smoothing static float value = 0.0; bool armPos = false; //checks if arm is on right (false) or left (true) void setup() { Serial.begin(115200); myServo.attach(SERVO_PIN); pinMode(TRIG_PIN, OUTPUT); pinMode(ECHO_PIN, INPUT); myServo.write(rightAngle); servoAngle = rightAngle; } void loop() { updateSonar(); servoRun(); delay(500); }
The above code shows the initial setup of the program along with the various global variables.
//receive input from sonar sensor and print to console void updateSonar(){ digitalWrite(TRIG_PIN, HIGH); delayMicroseconds(10); digitalWrite(TRIG_PIN, LOW); long pulseLen = pulseIn(ECHO_PIN, HIGH, 5900); long ping_time = pulseLen; //Serial.println(pulseLen); if (ping_time > 0){ //ensure boundaries for ping time if (ping_time >1200){ping_time=1200;} else if(ping_time <300){ping_time=300;} servoDelay = map(ping_time, 300, 1200, 200, 2000); servoDelay = smoothing(servoDelay, 0.35); } else{ servoDelay = 0; } Serial.println(servoDelay); } //First order smoothing filter with filtered input value of 0 float smoothing (float input, float coef){ float diff = input - value; value += coef*diff; return value; }
The above code shows how the sonar sensor was updated and how input was interpreted. The time for the sonar ping was mapped onto a new set of values for the servo motor delay in milliseconds. The second function shows the smoothing filter applied to reduce large spikes and changes in the servo’s behavior.
void servoRun(){ //runs if serial port has data if (Serial.available()){ recievedDelay = Serial.parseInt(); if (recievedDelay > 0){ //move servo arm to other side if (armPos) {servoAngle = rightAngle;} else {servoAngle = leftAngle;} myServo.write(servoAngle); delay(recievedDelay); } } }
The final function of the program moves the arm to the other side with the delay being received from the serial console.
Leave a Reply
You must be logged in to post a comment.