Hi, I have merged 3 simple codes which all worked by themselves but when combined the distance sensor values are all false and are likely random (though some stay consistent).
I'm quite new at arduino so there is likely stuff to do when combining codes that I don't know about,
1- sweeps with 2 servos
2-turns of and on a relay when object is close
3- measuring 4 distances with DYP L04 underwater sensors
Here is the code :
#include <SoftwareSerial.h>
#include <Servo.h>
//--------------------------------Initialization Block--------------------------------------------
// Initialization for distance measurement functions-----
#define COM1 0x55
#define COM2 0x56
#define COM3 0x57
#define COM4 0x58
int distance1 = 0;
int distance2 = 0;
int distance3 = 0;
int distance4 = 0;
unsigned char buffer_RTT[4];
uint8_t CS;
//------------------------------------------------------------------------
// Initialization for servo motors-----------------------------------
Servo myServo1; // Creates a servo object for controlling the servo motor
Servo myServo2; // Creates a servo object for controlling the servo motor
//------------------------------------------------------------------------
void setup() {
Serial.begin(115200);
myServo1.attach(4); // Defines the horizontal servo motor connected to pin 4
myServo2.attach(5); // Defines the vertical servo motor connected to pin 5
pinMode(3, OUTPUT); // Relay connected to pin 3
}
// --------------------------------------Main Loop-----------------------------------------------------------------
void loop() {
for (int i = 0; i < 160; i=i+3) { // Loop for horizontal movement
myServo1.write(i); // Sends the control pulse to the motor to set its angle
for (int j = 40; j <= 160; j=j+3){ // Loop to raise the servo motor
myServo2.write(j);
delay(30);
obstacle_detection(); // Calls the obstacle detection function during the upward movement of the sensor block (from left to right)
}
i=i+3; // Rotates the servo to the right
myServo1.write(i);
for (int j = 160; j > 40; j=j-3){ // Loop to lower the servo motor
myServo2.write(j);
delay(30);
obstacle_detection(); // Calls the obstacle detection function during the downward movement of the sensor block
}
}
for (int i = 160; i > 0; i=i-3) { //Loop to mirror what has been done above
myServo1.write(i);
for (int j = 40; j <= 160; j=j+3){ // Loop to raise the servo motor
myServo2.write(j);
delay(30);
obstacle_detection(); // Calls the obstacle detection function during the upward movement of the sensor block (from right to left)
}
i=i-3; // Rotates the servo to the left
myServo1.write(i);
for (int j = 160; j > 40; j=j-3){ // Loop to lower the servo motor
myServo2.write(j);
delay(30);
obstacle_detection(); // Calls the obstacle detection function during the upward movement of the sensor block (from right to left)
}
}
}
//---------------------------------------------------------------------------------------------------------------------------
//----------------- Obstacle Detection Function-------------------------
void obstacle_detection() {
//distance retrieval
int distance1bis = readSensor1();
Serial.print("Distance from Sensor 1: ");
Serial.print(distance1bis);
Serial.println(" mm");
int distance2bis = readSensor2();
Serial.print("Distance from Sensor 2: ");
Serial.print(distance2bis);
Serial.println(" mm");
int distance3bis = readSensor3();
Serial.print("Distance from Sensor 3: ");
Serial.print(distance3bis);
Serial.println(" mm");
int distance4bis = readSensor4();
Serial.print("Distance from Sensor 4: ");
Serial.print(distance4bis);
Serial.println(" mm");
//Action based on distance
if(distance1bis < 200 || distance2bis < 200 || distance3bis < 200|| distance4bis < 200 ){
if(distance1bis < 50 || distance2bis < 50 || distance3bis < 50|| distance4bis < 50 ){ // If we are at a critical distance
digitalWrite(3, HIGH); // Turns on the relay, cutting power to motors
delay(3000); // Wait a bit
digitalWrite(3, LOW); // Turns the motors back on
}
else { // If we are at a close but not critical distance, notify the user via vibration
for (int i = 0; i <= 200; i++){
digitalWrite(3, HIGH);
delay(50);
digitalWrite(3, LOW);
delay(50);
}
}
}
}
//------------------------------------------------------------------------
//------------------------ Distance Functions (BLOCK C) -----------------------
int readSensor1() {
SoftwareSerial mySerial1(53,52 );
mySerial1.begin(115200);
mySerial1.write(COM1);
delay(50);
if (mySerial1.available() > 0) {
delay(10);
if (mySerial1.read() == 0xff) {
buffer_RTT[0] = 0xff;
for (int i = 1; i < 4; i++) {
buffer_RTT[i] = mySerial1.read();
}
CS = buffer_RTT[0] + buffer_RTT[1] + buffer_RTT[2];
if (buffer_RTT[3] == CS) {
distance1 = (buffer_RTT[1] << 8) + buffer_RTT[2];
return distance1;
}
}
}
}
int readSensor2() {
SoftwareSerial mySerial2(49,48);
mySerial2.begin(115200);
mySerial2.write(COM2);
delay(50);
if (mySerial2.available() > 0) {
delay(10);
if (mySerial2.read() == 0xff) {
buffer_RTT[0] = 0xff;
for (int i = 1; i < 4; i++) {
buffer_RTT[i] = mySerial2.read();
}
CS = buffer_RTT[0] + buffer_RTT[1] + buffer_RTT[2];
if (buffer_RTT[3] == CS) {
distance2 = (buffer_RTT[1] << 8) + buffer_RTT[2];
return distance2;
}
}
}
}
int readSensor3() {
SoftwareSerial mySerial3(45,44);
mySerial3.begin(115200);
mySerial3.write(COM3);
delay(50);
if (mySerial3.available() > 0) {
delay(10);
if (mySerial3.read() == 0xff) {
buffer_RTT[0] = 0xff;
for (int i = 1; i < 4; i++) {
buffer_RTT[i] = mySerial3.read();
}
CS = buffer_RTT[0] + buffer_RTT[1] + buffer_RTT[2];
if (buffer_RTT[3] == CS) {
distance3 = (buffer_RTT[1] << 8) + buffer_RTT[2];
return distance3;
}
}
}
}
int readSensor4() {
SoftwareSerial mySerial4(39,38);
mySerial4.begin(115200);
mySerial4.write(COM4);
delay(50);
if (mySerial4.available() > 0) {
delay(10);
if (mySerial4.read() == 0xff) {
buffer_RTT[0] = 0xff;
for (int i = 1; i < 4; i++) {
buffer_RTT[i] = mySerial4.read();
}
CS = buffer_RTT[0] + buffer_RTT[1] + buffer_RTT[2];
if (buffer_RTT[3] == CS) {
distance4 = (buffer_RTT[1] << 8) + buffer_RTT[2];
return distance4;
}
}
}
}
//------------------------------------------------------------------------
The circuit is :
2 servos plugged in pwm on the pwm ports of the arduino
4 (3x 3m and 1x 6 meter range) distance sensors plugged into the Digitial ports
1 relay plugged in a digital port.
All power is provided through the arduino (directly plugged in through a breadboard)
I wasn't able to find power consumption of the sensors on datasheet.
With strictly nothing plugged into the arduino I get random distance values (I usually should get 0 when they aren't plugged in)
If the Mega worked before, doesn't now, download the blink.ino test code to make sure you didn't damage it with the two servos as loads
have you attached an external power supply for the servos, and if not, why not? This is an essential step
if you have added a power supply, make sure it's (-)(GND) side is connected to the Arduino's GND. This is necessary as the pulse outputs of the Arduino need to be referenced to the GND of both the Arduino, and the Servos.
If your Mega is working, I strongly suggest you focus on getting this working with the Mega, as there is no need for fussing around with Software Serial there - just use Serial1, Serial2, or Serial3, with appropriate pin selection of course.
Hope that sets you on the path to success.
On a mega you have 3 free hardware serial (Serial1 - Serial3). Please test with these. Serial however is shared with USB. For the last sensor you probably still need to use SoftwareSerial.
