Hello everybody,
I was software leader in team during engineering class, and I had not had any experience with writing code before. The goal of each team was to make robot, and then demonstrate how robot works. My team and I decided to make robot that would follow object in range from 20 cm to 60 cm. In fact, robot consisted of two DC motors, which allowed robot to move, servo motor, which rotated ultrasonic sensors and allowed them to increase its view angle, and ultrasonic sensors, which allowed robot to calculate distance between it and object. While hardware staff were under control of hardware leader, I just needed to write code for the robot. To write code that can follow object, I declared pins and maximum distance, made functions for DC motors and ultrasonic sensor, which allow me to save space, and made loop where the logic is located. I tested DC motors, servo motors, ultrasonic sensors, and functions, which were made to control them, with hardware leader, and they worked well. However, when I applied loop with the logic of the robot that consists mainly of if clauses my robot did not move but it could rotate ultrasonic sensors, and it could calculate distances between robot and object. Lemme clarify how logic works, if front distance is equal to range, it moves forward, otherwise if the front distance is not located in the range, we ask our robot to look at right/left, and if right/left distance is equal to the range, our robot will move right/left, otherwise if right/left distance is more than 60 cm, it will stay, or if right/left distance is less than 20cm, the robot will move backwards. As I mentioned before, I think that problem is located in loop but I could not find it because all functions worked well separately. Hence, I would really appreciate if someone could help me with solving my problem.
Here is a code
#include <NewPing.h> // is used to work with ultrasonic sensor
#include <Servo.h> // is used for servo motor
#define TRIG_PIN 12
#define ECHO_PIN 11
#define MAX_DISTANCE 200
Servo myservo; //initialize a servo object for the connected servo
int angle = 0; // Declaring starting angle
int in1 = 7; //Declaring the pins where in1 in2 from the driver are wired
int in2 = 8; // in1 and in2 are used for motor A
int in3 = 2; // Declaring the pins where in3 in4 from the driver are wired
int in4 = 13; // in2 and in3 are used for motor B
int ConA = 5; // PWM signal for A (First DC motor)
int ConB = 3; // PWM signal for B (Second DC motor)
// PWM signal is used to control the speed motor, and only pins with this symbol “~” can deliver PWM signal
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);
int readPing() {
delay(70);
int cm = sonar.ping_cm();
}
// The function below is used to turn on motor B, the wheel rotates counterclockwise in order to go forward
void TurnMotorB(){
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
analogWrite(ConB,100); //Speed range (0-255)
}
// The function below is used to turn on motor A, the wheel rotates counterclockwise in order to go forward
void TurnMotorA(){
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
analogWrite(ConA, 100); //Speed range (0-255)
}
// The function below is used to turn off motor A, in other words, it used to stop motor A
void TurnOFFA(){
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
analogWrite(ConA, 0);
}
// The function below is used to turn off motor B, in other words, it used to stop motor B
void TurnOFFB(){
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
analogWrite(ConB,0);
}
// The function below is used to rotate motor A clockwise, in other words, it used to move backward
void TurnBackA(){
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
analogWrite(ConA, 50); //Speed range (0-255)
}
// The function below is used to rotate motor B clockwise, in other words, it used to move backward
void TurnBackB(){
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
analogWrite(ConB, 50); //Speed range (0-255)
}
void setup() {
myservo.attach(9); // it makes ultrasonic sensors to look forward
myservo.write(115);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
pinMode(ConB, OUTPUT);
pinMode(ConA, OUTPUT);
}
void loop() {
// Before we declared that robot’s ultrasonic sensors would look forward in the beginning, that is why left, and right distance is equal to zero
int RightDistance = 0;
int LeftDistance = 0;
int FrontDistance = readPing(); //this line asks our robot to measure the distance in front of the robot
delay(100);
if (FrontDistance > 20 && FrontDistance < 60) {
TurnMotorA();
TurnMotorB();
}
else if (FrontDistance < 20 ) {
myservo.write(170);
delay(1000);
int RightDistance = readPing();
delay(100);
myservo.write(115);
if (RightDistance > 20 && RightDistance < 60){
TurnOFFB();
TurnMotorA();
} else {
myservo.write(60);
delay(1000);
int LeftDistance = readPing();
delay(100);
myservo.write(115);
} if (LeftDistance > 20 && LeftDistance < 60) {
TurnOFFA();
TurnMotorB();
}else{
TurnBackB();
TurnBackA();
}
}
else if (FrontDistance > 60){
myservo.write(170);
delay(1000);
int RightDistance = readPing();
delay(100);
myservo.write(115);
if (RightDistance > 20 && RightDistance < 60){
TurnOFFB();
TurnMotorA();
} else {
myservo.write(60);
delay(1000);
int LeftDistance = readPing();
delay(100);
myservo.write(115);
} if (LeftDistance> 20 && LeftDistance < 60) {
TurnOFFA();
TurnMotorB();
}else{
TurnOFFA();
TurnOFFB();
}
}
}
