Hello.. i wrote a code that uses this library, and isn't 100% reliable when debugging...
I'd be grateful if you comment it.
#include <NewPing.h>
#define BAUD 9600
#define MIN_DISTANCE 6 //Minimum distance to ping (cm)
#define MAX_DISTANCE 60 //Maximum distance to ping (cm)
#define THRESHOLD 10 //Treshold (cm)
#define HOLD_TIME 15 //Delay between each step (ms)
#define STEP_TIME 33 //Delay between ultrasonic's
//Creating ultrasonic objects
NewPing SONAR_L(TRIG_PIN_L, ECHO_PIN_L, 2000);
NewPing SONAR_R(TRIG_PIN_R, ECHO_PIN_R, 2000);
//Creating debugg function
void debugg(int val_l, int val_r)
{
Serial.print(val_l);
Serial.print(" ");
Serial.print(val_r);
Serial.println();
}
//Creating setup function
void setup()
{
Serial.begin(BAUD);
pinMode(ECHO_PIN_L, INPUT);
pinMode(ECHO_PIN_R, INPUT);
pinMode(TRIG_PIN_L, OUTPUT);
pinMode(TRIG_PIN_R, OUTPUT);
}
//Program
void loop()
{
//Declaring local static variables
static bool hold = false;
static int val_l, val_r;
static int per_l, per_r;
static int prev_per_l = MIN_DISTANCE, prev_per_r = MIN_DISTANCE;
//Reading ultrasonic values
delay(STEP_TIME);
unsigned int uS1 = SONAR_L.ping();
per_l = uS1 / US_ROUNDTRIP_CM;
delay(STEP_TIME);
unsigned int uS2 = SONAR_R.ping();
per_r = uS2 / US_ROUNDTRIP_CM;
//Limiting the effective range of ultrasonics
per_l = constrain(per_l, MIN_DISTANCE, MAX_DISTANCE);
per_r = constrain(per_r, MIN_DISTANCE, MAX_DISTANCE);
//This part of code is used to save the distance of ultrasonic sensors
//when the hand is moved away from the beam ) ) ) of the sensor
if( (per_l - prev_per_l) > THRESHOLD )
{
per_l = prev_per_l;
hold=false;
}
if( (per_r - prev_per_r) > THRESHOLD )
{
per_r = prev_per_r;
hold=false;
}
prev_per_l = per_l;
prev_per_r = per_r;
//Modifying the data using linear math (used for servo motors)
per_l = map(per_l, MIN_DISTANCE, MAX_DISTANCE, 100, 0);
val_l = map(per_r, MIN_DISTANCE, MAX_DISTANCE, 90, 0);
val_r = map(per_r, MIN_DISTANCE, MAX_DISTANCE, 0, 90);
val_l = val_l * (per_l / 100.0);
val_r = val_r * (per_l / 100.0);
//Writing final data to servo motors
servo_l.write(val_l);
servo_r.write(val_r);
//Debugging the final data
debugg(val_l, val_r);
//Applying the final delay (due to servo speed characteristics)
hold=true;
if(hold==true) delay(HOLD_TIME);
}