I am writing a Arduino script for my Obstacle avoiding Car... It should scan the area for obstacles using both ultrasonics and DHT11 (by calculating speed of sound) to avoid hitting it and go anywhere else, and if there are obstacles everywhere, it should trigger a buzzer with a frequency of 70mHz every 3 seconds for 1 second...
- I can't get the motors working
- the Ultrasonic seems to be detecting everywhere as obstacle
- the Servo rotates 360" not 180" for some reason (which broke my servo. After struggling to move/rotate, the top gear got broken...)
#include <DHT_U.h>
#include <DHT.h>
#include <Servo.h>
#define MOVE_DIRECTION_FORWARD (0)
#define MOVE_DIRECTION_BACKWARD (1)
#define MOVE_DIRECTION_RIGHT (2)
#define MOVE_DIRECTION_LEFT (3)
#define MOVE_DIRECTION_NONE (4)
Servo zServo;
const int trigPin = 9;
const int echoPin = 10;
const int servoPin = 2;
const int dhtPin = 7;
const int buzzerPin = 8;
const int leftAPin = 3;
const int leftBPin = 4;
const int rightAPin = 5;
const int rightBPin = 6;
DHT_Unified dht(dhtPin, DHT11);
#define SAFE_DIST_CM 3
const int SCAN_ANGLES[] = {0,15,30,45,60,75,90,105,120,135,150,165,180};
const int NUM_SECTORS = sizeof(SCAN_ANGLES)/sizeof(int);
unsigned long curMovementDirection = MOVE_DIRECTION_NONE;
int distances[NUM_SECTORS];
unsigned long lastBuzzerMillis = 0;
bool buzzerOn = false;
bool commandMode = false;
unsigned long lastScan = 0;
void setup() {
zServo.attach(servoPin);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(leftAPin, OUTPUT);
pinMode(leftBPin, OUTPUT);
pinMode(rightAPin, OUTPUT);
pinMode(rightBPin, OUTPUT);
pinMode(buzzerPin, OUTPUT);
dht.begin();
Serial.begin(115200);
zServo.write(90);
zServo.writeMicroseconds(1500);
}
int measureDistance(bool unitInch = false, bool enbDHT = false)
{
float duration, distance;
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = (float)pulseIn(echoPin, HIGH);
if(enbDHT == false)
distance = duration * (unitInch ? 0.0133 : 0.034) / 2;
else
{
sensors_event_t DHTsensor[2];
dht.temperature().getEvent(&DHTsensor[0]);
dht.humidity().getEvent(&DHTsensor[1]);
if(isnan(DHTsensor[0].temperature) || isnan(DHTsensor[1].relative_humidity))
{
Serial.println("ERROR: reading DHT11 sensor");
return -1;
}
float speedOfSound = 331.4 + (0.6 * DHTsensor[0].temperature) + (0.0124 * DHTsensor[1].relative_humidity);
duration = duration/1e6f;
distance = (speedOfSound * duration)/2.0f;
distance = distance * 100;
if(unitInch)
distance = distance/2.54;
}
return distance;
}
void MoveForward()
{
if(curMovementDirection == MOVE_DIRECTION_FORWARD)
return;
curMovementDirection = MOVE_DIRECTION_FORWARD;
digitalWrite(leftAPin, HIGH);
digitalWrite(leftBPin, LOW);
digitalWrite(rightAPin, HIGH);
digitalWrite(rightBPin, LOW);
Serial.println("DEBUG: Moving forward.");
}
void MoveBackward()
{
if(curMovementDirection == MOVE_DIRECTION_BACKWARD)
return;
curMovementDirection = MOVE_DIRECTION_BACKWARD;
digitalWrite(leftAPin, LOW);
digitalWrite(leftBPin, HIGH);
digitalWrite(rightAPin, LOW);
digitalWrite(rightBPin, HIGH);
Serial.println("DEBUG: Moving backward.");
}
void turnInPlace(float angleDeg) {
bool dirCW = (angleDeg > 0);
digitalWrite(leftAPin, dirCW);
digitalWrite(leftBPin, !dirCW);
digitalWrite(rightAPin, !dirCW);
digitalWrite(rightBPin, dirCW);
int dur = abs(angleDeg) * 10;
delay(dur);
StopMoving();
}
void StopMoving()
{
if(curMovementDirection == MOVE_DIRECTION_NONE)
return;
curMovementDirection = MOVE_DIRECTION_NONE;
digitalWrite(leftAPin, LOW);
digitalWrite(leftBPin, LOW);
digitalWrite(rightAPin, LOW);
digitalWrite(rightBPin, LOW);
Serial.println("DEBUG: stopped movement.");
}
void scanEnvironment() {
for (int i = 0; i < NUM_SECTORS; i++) {
if(0 <= SCAN_ANGLES[i] <= 180)
{
zServo.write(SCAN_ANGLES[i]);
delay(200);
distances[i] = measureDistance(false, true);
char msg[70];
sprintf(msg, "DEBUG: picked angle %d (sector %d)", distances[i], i);
Serial.println(msg);
}
}
}
int pickBestSector() {
int bestIdx = -1;
int maxDist = -1;
for (int i = 0; i < NUM_SECTORS; i++) {
if (distances[i] > maxDist) {
maxDist = distances[i];
bestIdx = i;
}
}
char msg[70];
sprintf(msg, "DEBUG: picked best sector %d", bestIdx);
Serial.println(msg);
return bestIdx;
}
bool allBlocked() {
for (int i = 0; i < NUM_SECTORS; i++) {
if (distances[i] > SAFE_DIST_CM) return false;
}
return true;
}
void handleBuzzer() {
unsigned long now = millis();
if (buzzerOn) {
if (now - lastBuzzerMillis >= 1000) {
noTone(buzzerPin);
buzzerOn = false;
lastBuzzerMillis = now;
Serial.println("DEBUG: Buzzer OFF");
}
} else {
if (now - lastBuzzerMillis >= 3000) {
tone(buzzerPin, 70);
buzzerOn = true;
lastBuzzerMillis = now;
Serial.println("DEBUG: Buzzer ON");
}
}
}
void loop() {
if (millis() - lastScan > 500) {
scanEnvironment();
lastScan = millis();
}
if (allBlocked()) {
Serial.println("DEBUG: All sectors blocked!");
curMovementDirection = MOVE_DIRECTION_BACKWARD;
MoveBackward();
delay(500);
StopMoving();
commandMode = true;
lastBuzzerMillis = millis();
buzzerOn = false;
return;
}
if (commandMode) {
handleBuzzer();
return;
}
int bestIdx = pickBestSector();
if(bestIdx != -1)
{
int sectorAngle = SCAN_ANGLES[bestIdx];
float bestAngle = sectorAngle - 90.0;
if (bestAngle < 0) {
if(curMovementDirection == MOVE_DIRECTION_LEFT)
return;
curMovementDirection = MOVE_DIRECTION_LEFT;
turnInPlace(bestAngle);
Serial.println("DEBUG: Moving Left.");
return;
} else if (bestAngle > 0) {
if(curMovementDirection == MOVE_DIRECTION_RIGHT)
return;
curMovementDirection = MOVE_DIRECTION_RIGHT;
Serial.println("DEBUG: Moving Right.");
turnInPlace(bestAngle);
return;
} else {
curMovementDirection = MOVE_DIRECTION_FORWARD;
MoveForward();
return;
}
}
Serial.println("DEBUG: No best sector found (this should ideally not happen).");
delay(400);
StopMoving();
}
