Please help! We're trying to build a car while using a bluetooth module HC-05, ultrasonic sensor and servo to test all the angles and a motor driver. This is my code and it's not working. It might have something to do with the user defined functions?
thanks!
the motors don't work when running this code, so that's why we think it might have something to do with how we pass values. Each part works independently.
//Libraries
#include <Servo.h>
Servo myservo_test; //Class to manipulate servo's
//Variable Initialization
String voice; // initialize the inputs and motors to arduino pins
int pos=0; //initialize angle to 0, this is the position of the servo
//Initialization of Motor Variables
const int enA = 9;
const int enB = 3;
const int in1 = 7;
const int in2 = 6;
const int in3 = 5;
const int in4 = 4;
//Initialization of Ultrasonic Sensor Pins & Variables
const int echo = 10;//Echo pins is responsible to hear the sound
const int trig = 8; //The trigger of sound, basically when it's on low it's off but when it's on high it will emit a sound until off
double distance; //distance will be in millimeters, should be double to allow for decimals
double time; //double to allow for decimals
//USER DEFINED FUNCTIONS
void UltraFunc(String& direct) //Returns values of direct to voice, must be a different variable name because voice doesn't exist in the scope of this void function
{
double sum=0;
double average=0;
for (int i =1; i <=3; i++)
{
digitalWrite(trig, LOW); //Reinitializes trig to LOW which is the value for OFF or false
delay(2); // Turns trig off for two seconds
digitalWrite(trig, HIGH);
delay(20); // turns it back on for 10 seconds
digitalWrite(trig, LOW); // turns trig back off
time = pulseIn(echo, HIGH); // turns on the echo pin which hears for the sound
// Calculating the distance by finding the speed that sound travels and **continue later
distance= time*0.1715; //Because 343m/s is the speed of sound, and the distance is divided by 2 because it is the time it takes for one sound emitted to hit the wall and come back, and it is in mm. The sound is divided by 2 because the sound is emitted by the TRIG pin for 2 seconds, every ten seconds.
Serial.print("Test "); //3 tries
Serial.print (i);
Serial.print(", Distance: ");
Serial.print(distance); // this will print the distance
Serial.println(" mm");
sum=sum+distance;// summation all of distances
average=sum/(double)i;//average to eliminate inaccuracy
}//Tests 3 times to see if there's anything in front in case of error // end of for-loop
if (average <= 1700) //if the distance between the car and some object less than 1700mm, or 17cm (which is the length of the car is 16cm which is the distance that is required to turn without hitting anything with 1 cm room for error)
direct="stop"; //return command stop so the arduino can re-evaluate
} //end of UltraFunc function
void servoFunc(String& change) //new variable that will change direct if there is anything that is scanned.
{
if (change=="right") // voice = direct= change, if the car is going right then the servo must turn right and scan if anything is turning before turning right. It must check in front of it and in the direction of motion
{
for (pos = 25; pos <= 90; pos ++) //Servo scans between 25 degree position to 90 and back, it stops at 50 degrees too
{
myservo_test.write(pos);
delay(15);
UltraFunc(change);
}
for (pos = 90; pos <= 25; pos --) //Servo scans between 25 degree position to 90 and back, it stops at 50 degrees too
{
myservo_test.write(pos);
delay(15);
UltraFunc(change);
}
} // end of if right statement
if (change=="left") // voice = direct= change, if the car is going right then the servo must turn right and scan if anything is turning before turning left. It must check in front of it and in the direction of motion
{
for (pos = 100; pos <= 145; pos ++)
{
myservo_test.write(pos);
delay(15);
UltraFunc(change);
}
} // end of if left
for (pos = 270; pos <= 100; pos --)
{
myservo_test.write(pos);
delay(15);
UltraFunc(change);
}
if (change == "stop") //If at any point during the rotations there was something within 1700mm then the car will stop.
Serial.println("There is something in the way");
}//end of servo void function
void setup()
{
//Sets the motors as output
pinMode(enA, OUTPUT); //set motor control pins to outputs, motor 1
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(enB, OUTPUT); //motor 2
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
//Sets the Ultrasonic Sensor echo and trig as output and/or input values
pinMode(trig, OUTPUT); //It sets a pin to be either an Input or Output, where here trig is output
pinMode(echo, INPUT); // Sets the echo as an Input
//Initialization of Servo
myservo_test.attach(12); // attach the signal pin of servo to pin 12 of arduino // attach is found in the library for servo so that the servo is connected to those pins
//Wiring: Red wire VCC, Brown GRND, Orange 12
Serial.begin(9600);// Starts the communication with the serial monitor at 9600 bits per second-baud value, must be 9600 because that is the baud value of this arduino Uno kit
}
void loop()
{
//Receives audio input from HC-05 Bluetooth module connected to Android phone, it can also receive commands by a terminal on the phone
if(Serial.available()) // serial.available tests to seee how many bytes are available
{
voice = Serial.readString();
delay(0.01);
Serial.println(voice);
}
//Scans to see if there is anything in the way
myservo_test.write(90); //Reserts the angle directly infront
UltraFunc(voice); // will scan directly in front
servoFunc(voice); //enters servoFunc and will take voice variable with it. It will test to see if there is anything in front of it to the left or to the right and it will stop the car if there is.
if(voice == "forward"){
analogWrite(enA, 255);
analogWrite(enB, 255);
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
}
else if(voice == "reverse"){
analogWrite(enA, 255);
analogWrite(enB, 255);
digitalWrite(in1, HIGH); // Motor A
digitalWrite(in2, LOW); //Motor A
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
}
if(voice == "right"){
analogWrite(enA, 255);
analogWrite(enB, 255);
digitalWrite(in1, LOW); // Motor A
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
}
else if(voice == "left") {
analogWrite(enA, 255);
analogWrite(enB, 255);
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
}
if(voice == "stop"){
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
}
} //end of loop
