just as i said, I'm trying to control a servo with sensor readings, but it wont work. the if statement with all the servo code isnt acting how it should and instead keeps on playing even when distance goes above 20, and the servo will just eternally and continually spin in one direction once the if statement becomes true. help
#include <Servo.h>
#define servoPin 7
#define TRIG_PIN 3 // TRIG pin
#define ECHO_PIN 2 // ECHO pin
int angle = 0;
Servo myservo;
float filterArray[20]; // array to store data samples from sensor
float distance; // store the distance from sensor
void setup() {
// begin serial port
Serial.begin (9600);
myservo.attach(servoPin);
// configure the trigger and echo pins to output mode
pinMode(TRIG_PIN, OUTPUT);
pinMode(ECHO_PIN, INPUT);
}
void loop() {
// 1. TAKING MULTIPLE MEASUREMENTS AND STORE IN AN ARRAY
for (int sample = 0; sample < 20; sample++) {
filterArray[sample] = ultrasonicMeasure();
delay(30); // to avoid untrasonic interfering
}
// 2. SORTING THE ARRAY IN ASCENDING ORDER
for (int i = 0; i < 19; i++) {
for (int j = i + 1; j < 20; j++) {
if (filterArray[i] > filterArray[j]) {
float swap = filterArray[i];
filterArray[i] = filterArray[j];
filterArray[j] = swap;
}
}
}
// 3. FILTERING NOISE
// + the five smallest samples are considered as noise -> ignore it
// + the five biggest samples are considered as noise -> ignore it
// ----------------------------------------------------------------
// => get average of the 10 middle samples (from 5th to 14th)
double sum = 0;
for (int sample = 5; sample < 15; sample++) {
sum += filterArray[sample];
}
distance = sum / 10;
// print the value to Serial Monitor
Serial.print("distance: ");
Serial.print(distance);
Serial.println(" cm");
}
float ultrasonicMeasure() {
// generate 10-microsecond pulse to TRIG pin
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_PIN, LOW);
// measure duration of pulse from ECHO pin
float duration_us = pulseIn(ECHO_PIN, HIGH);
// calculate the distance
float distance_cm = 0.017 * duration_us;
if (distance_cm < 20)
{
myservo.write(90);
delay(1000);
myservo.write(180);
delay(1000);
myservo.write(0);
delay(1000);
// Sweep from 0 to 180 degrees:
for (angle = 0; angle <= 180; angle += 1)
{
myservo.write(angle);
delay(15);
}
// And back from 180 to 0 degrees:
for (angle = 180; angle >= 0; angle -= 1)
{
myservo.write(angle);
delay(30);
}
delay(1000);
}
return distance_cm;
}
The best approach is to test each section by itself using the serial output printing to the IDE monitor to understand what is going on inside your program.
For instance, in your measure routing, print the raw "duration_us". Note the Arduino C++ does not print floats with converting to string form. So its best to print the data before the calculations create a float.
Its always better to print out the Raw data. Then its easy to go back and check the math knowing the input data is correct.
BTW although it may seem like the if statements don't work. Be assured they are doing exactly what you told them to do. Sometimes its frustrating for me when troubleshooting an issue.
If it turns more than 300 degrees in one direction, it is a hobby servo modified for continuous rotation and can't be moved to any particular position. From the "eternally and continually spin in one direction" that seems to be the case.
If a 'servo' EVER goes more than 360° in one direction it is a "continuous rotation" servo and can't be positioned to any particular angle.
(The one exception is a very expensive "Winch Servo" that is used mostly on RC sailing boats. It can be positioned anywhere in a typically ten-turn range.)
This is not a very good description of what you want to code to actually do. If you could be more detailed about the response of the servo based on the input of the sensor that would be helpful.
It seems odd to me that you are trying to control the servo in the ultrasonicMeasure() function when you have gone to the trouble of filtering the measurements.
Look at your code closely. Especially this loop:
for (int sample = 0; sample < 20; sample++) {
filterArray[sample] = ultrasonicMeasure();
delay(30); // to avoid untrasonic interfering
}
If distance_cm in ultrasonicMeasure() is less than 20 then the above loop could potentially take >480 seconds (8 minutes)!!! Why? Because when distance_cm in ultrasonicMeasure() is less than 20 then it takes ultrasonicMeasure() 12.1 seconds to execute.
if (distance_cm < 20)
{
myservo.write(90);
delay(1000);
myservo.write(180);
delay(1000);
myservo.write(0);
delay(1000);
// Sweep from 0 to 180 degrees:
for (angle = 0; angle <= 180; angle += 1)
{
myservo.write(angle);
delay(15);
}
// And back from 180 to 0 degrees:
for (angle = 180; angle >= 0; angle -= 1)
{
myservo.write(angle);
delay(30);
}
delay(1000);
}