Hello everyone,
I am working on a robotic car to check the surroundings by using ultrasonic sensor with servo motor with 180 degree movement. The problem is that the readings from the sensor is not stable as the servo rotates, I will attach the code and a screen shot of a sample of the readings. The car stops every 6 seconds to check the surroundings and then complete the movement.
#include <AFMotor.h>
#include <Servo.h>
AF_DCMotor TR(1, MOTOR12_64KHZ);
AF_DCMotor TL(2, MOTOR12_64KHZ);
AF_DCMotor BL(3, MOTOR12_64KHZ);
AF_DCMotor BR(4, MOTOR12_64KHZ);
int servo1 = 9;
int servo2 = 10;
int IR_R = 14;
int IR_L = 15;
unsigned long T1 = 0;
unsigned long T2 = 0;
Servo servo_sensor;
Servo servo_camera;
int trigPin = 17;
int echoPin = 16;
long duration, cm, inches;
void setup() {
servo_sensor.attach(9);
servo_camera.attach(10);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
Serial.begin(9600);
TR.setSpeed(100);
TL.setSpeed(100);
BR.setSpeed(100);
BL.setSpeed(100);
pinMode(IR_R,INPUT);
pinMode(IR_L,INPUT);
servo_sensor.write(0);
}
void loop() {
T1 = millis();
if (T1 > T2 + 6000){
TR.run(RELEASE);
TL.run(RELEASE);
BR.run(RELEASE);
BL.run(RELEASE);
for(int i=0;i<=180;i=i+3){
servo_sensor.write(i);
ultra();
delay(50);
Serial.print(cm);
Serial.print("cm");
Serial.println();
}
for(int i=180;i>=0;i=i-3){
servo_sensor.write(i);
ultra();
delay(50);
Serial.print(cm);
Serial.print("cm");
Serial.println();
}
T2 = T1; }
if (digitalRead(IR_R)==0 && digitalRead(IR_L) ==0){
TR.run(FORWARD);
TL.run(FORWARD);
BR.run(FORWARD);
BL.run(FORWARD);
}
else if (digitalRead(IR_R)==1 && digitalRead(IR_L) ==0){
TR.run(BACKWARD);
TL.run(FORWARD);
BR.run(BACKWARD);
BL.run(FORWARD);
}
else if (digitalRead(IR_R)==0 && digitalRead(IR_L) ==1){
TR.run(FORWARD);
TL.run(BACKWARD);
BR.run(FORWARD);
BL.run(BACKWARD);
}
}
void ultra (){
digitalWrite(trigPin,LOW);
delayMicroseconds(5);
digitalWrite(trigPin,HIGH);
delayMicroseconds(10);
digitalWrite(trigPin,LOW);
pinMode(echoPin,INPUT);
duration = pulseIn(echoPin,HIGH);
cm = (duration/2) / 29.1;
}