code robot
// Define pins for H-bridge
#define enA 6
#define enB 9
#define IN1 2
#define IN2 3
#define IN3 4
#define IN4 5
// Define pins for ultrasonic sensor
#define trigPin 7
#define echoPin 8
// Define pins for IR sensors
#define IRSensorR 10
#define IRSensorRM 11
#define IRSensorLM 12
#define IRSensorL 13
int IRSensorValues; // Integer value to store the values of the sensors as bits in 1 value
// Booleans to indicate if a sharp right or sharp left turn is detected
bool sharpright = false;
bool sharpleft = false;
bool turnAround = false; // Boolean to indicate if the robot is turning around to be able to disable the collision detection
static unsigned long startTime; // Variable to store the startTime of the timer, which is used for the sharp turns and for turning at the end of the line
// Variables to store the information of the ultrasonic sensor
long duration;
int distance;
int obstacleDistance = 10; //in cm
// To compensate the deviation of the motors(motor B is driving faster than motor A), we limited the maximum speed of motor B
float maxSpeedMotorA = 255;
float maxSpeedMotorB = 190;
float motorSpeed = 0.6; // Variable to control the total speed of the robot
// Variables to store the actual speed per motor
float speedMotorA;
float speedMotorB;
void setup() {
// The 4 pins for the IR sensors are set to input
pinMode(IRSensorR,INPUT);
pinMode(IRSensorRM,INPUT);
pinMode(IRSensorLM,INPUT);
pinMode(IRSensorL,INPUT);
// The 6 pins for the H-bridge are set to output
// To control the speed and the direction of motor 1
pinMode(enA, OUTPUT);
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
// To control the speed and the direction of motor 2
pinMode(enB, OUTPUT);
pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
// The 2 pins for the input and output are set for the ultrasonic sensor
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
// Calculate the speed per motor motor according to the max speed per motor and the total speed of the motors
speedMotorA = maxSpeedMotorA * motorSpeed;
speedMotorB = maxSpeedMotorB * motorSpeed;
delay(5000); //startup delay so the robot doesn't drive immediately after uploading the code
}
void loop() {
delay(50); // Small delay to be sure the robot can process the sensor values completely
// When the ultrasonic sensor detects an obstacle and the robot is not turning around, the robot needs to stop
// otherwise, the robot needs to follow the line
if(collisionDetected() && !turnAround){
wheels(0,0);
} else{
lineDetection();
}
}
void lineDetection(){
// Put the 4 values of the IR sensors in 1 integer to be able to use the switch case statement and make it more readable
// High sensor value means line detected, low sensor value means no line detected
IRSensorValues = B00000000;
IRSensorValues += B1 * digitalRead(IRSensorR);
IRSensorValues += B10 * digitalRead(IRSensorRM);
IRSensorValues += B100 * digitalRead(IRSensorLM);
IRSensorValues += B1000 * digitalRead(IRSensorL);
// Switch case to determine direction of the motors according to the sensor values
switch (IRSensorValues){
case B0110:
wheels(1,1);
resetBools(); // The sensors detect a straight line, so booleans for sharpturns needs to be resetted
break;
case B1111:
wheels(1,1);
break;
case B1110:
// There is a sharp left turn coming, so the boolean needs to be true if there wasn't already a sharp right turn detected
if(!sharpright){
sharpleft = true;
}
wheels(0.5,1);
break;
case B0111:
// There is a sharp right turn coming, so the boolean needs to be true if there wasn't already a sharp left turn detected
if(!sharpleft){
sharpright = true;
}
wheels(1,0.5);
break;
case B1000:
wheels(-1,1);
break;
case B1100:
wheels(0.5,1);
break;
case B0100:
wheels(0,1);
break;
case B0010:
wheels(1,0);
break;
case B0011:
wheels(1,0.5);
break;
case B0001:
wheels(1,-1);
break;
case B0000:
// When there is no line detected anymore, drive forward until the time has passed in 1 of the 3 cases
wheels(1,1);
// When a sharp left turn was detected, turn left after 500 ms
if(sharpleft){
if(millis() - startTime > 500){
wheels(-1,1);
}
}
// When a sharp right turn was detected, turn right after 500 ms
else if(sharpright){
if(millis() - startTime > 500){
wheels(1,-1);
}
} else{
// When no sharp turn was detected, stop after 1500 ms and turn around after 2000 ms
if(millis() - startTime > 1500){
if(millis() - startTime < 2000){
wheels(0,0);
} else{
wheels(1,-1);
turnAround = true; // Set the turnAround boolean to true so the ultrasonic sensor is disabled during turning
}
}
}
break;
default: // When sensor input is not known/possible, stop the motors
wheels(0,0);
break;
}
// When the line is detected by at least 1 of the sensors, reset the timer and set the turnAround boolean to false
// so the robots will conitinue following the line
if(IRSensorValues != B0000){
startTime = millis();
turnAround = false;
}
}
// Function to reset the bools to be sure the robot is not turning when it doesn't need to
void resetBools(){
sharpleft = false;
sharpright = false;
}
// Function to set a boolean value whether an obstacle is detected or not
bool collisionDetected(){
// Clears the trigPin
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
// Sets the trigPin on HIGH state for 10 micro seconds
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// Reads the echoPin, returns the sound wave travel time in microseconds
duration = pulseIn(echoPin, HIGH);
// Calculating the distance
distance = duration*0.034/2;
// When an object is closer than the specified obstacle distance, return true, otherwise return false
return (distance < obstacleDistance);
}
// Function to control the wheels, positive value means forward, negative value means backward and 0 means stop
// Values between 0 and 1 can be used to control the speed per motor. 0.5 means that the motor is running on half power
void wheels(float directionLeft, float directionRight){
// Right Motor(A)
if(directionRight > 0){ // Forward
analogWrite(enA, directionRight*speedMotorA);
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
} else if(directionRight < 0){ // Reverse
analogWrite(enA, -directionRight*speedMotorA);
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
} else{ // Stop
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
}
// Left Motor(B)
if(directionLeft > 0){ // Forward
analogWrite(enB, directionLeft*speedMotorB);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
} else if(directionLeft < 0){ // Reverse
analogWrite(enB, -directionLeft*speedMotorB);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
} else{ // Stop
digitalWrite(IN3, LOW);
digitalWrite(IN4, LOW);
}
}