Code for ultrasonic sensor car / robotics car kit

stekoel · September 11, 2023, 5:58pm

[NOTE: UPDATED CODE IN THE OTHER POST FURTHER BELOW]
Hi all, I have seen that there were posts on Arduino-code for the ultrasonic sensor car / robotics car kit 87288 / Robotbil - startpakke, that were closed although not really solved. Please find some code that works below.
Note the following issues:

the other posts I found in this forum used digitalWrite to control the motors, but with analogWrite it is also possible to control the speed. But then you have to make sure that the correct pins are used to control the motors. I have used the PWM pins 6 and 11 for the left motor/wheel, and 3 and 5 for the right motor/wheel (these pins (use "S" row) should then connect to the IN1 to IN4 pins on the motor board L298N). I connected the servo-motor to pin-column 12 (make sure that the orange cable connects to the "S" pin of that column).
I set the default motor speed to "100" but you can set it up to 255.
One of my motors was broken (the motor was turning, but not the wheel), which made the robot spin when the motors turned on. (Had to exchange it.) Another user also reported this problem in their post. So if the robot is just spinning then double-check the motors.
My left motor is running slightly slower than the right motor, so to prevent the robot from driving with a slight leftward-bias I added a bit speed when driving forward, that is why one of the lines towards the end includes "speedSetting + 10".

I do hope that this code will work for you and that it will provide you with a nice basis to develop your robotic car robot further!

#include <Servo.h>

Servo myservo;  // Create servo object

int pinLF = 6;  // Set to control the left forward wheel
int pinLB = 11;  // Set to control the left backward wheel
int pinRF = 3;  // Set to control the right forward wheel
int pinRB = 5;  // Set to control the right backward wheel

int trigPin = A1;  // Trigger Pin of Ultrasonic Sensor
int echoPin = A0;  // Echo Pin of Ultrasonic Sensor

int speedSetting = 100; // Motor speed

void setup() {
  pinMode(pinLF, OUTPUT);
  pinMode(pinLB, OUTPUT);
  pinMode(pinRF, OUTPUT);
  pinMode(pinRB, OUTPUT);

  pinMode(trigPin, OUTPUT); // Sets the trigPin as an OUTPUT
  pinMode(echoPin, INPUT); // Sets the echoPin as an INPUT

  myservo.attach(12);  // Attach servo on pin 12

}

int readDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  return duration * 0.034 / 2;
}


void loop() {
  int distance = readDistance();
  
  if (distance < 40) {
    // Stop
    digitalWrite(pinLF, LOW);
    digitalWrite(pinLB, LOW);
    digitalWrite(pinRF, LOW);
    digitalWrite(pinRB, LOW);
    
    // Measure to the left
    myservo.write(140);
    delay(500);
    int leftDistance = readDistance();

    // Measure to the right
    myservo.write(40);
    delay(500);
    int rightDistance = readDistance();
    
    // Reset servo to center
    myservo.write(90); 
    
// if there are less than 30 cm in either direction then reverse for 1.5 second and stop. 
if (leftDistance < 25 || rightDistance < 25){
    analogWrite(pinLF, 0);
    analogWrite(pinLB, speedSetting);
    analogWrite(pinRF, 0);
    analogWrite(pinRB, speedSetting);
    delay(1500); 
    // Stop
    digitalWrite(pinLF, LOW);
    digitalWrite(pinLB, LOW);
    digitalWrite(pinRF, LOW);
    digitalWrite(pinRB, LOW);
    delay(200); 
}

    // Measure again to the left
    myservo.write(140);
    delay(500);
    leftDistance = readDistance();

    // Measure again to the right
    myservo.write(40);
    delay(500);
    rightDistance = readDistance();

    // Reset servo to center
    myservo.write(90); 

    
    // Turn in the direction with more space
    if (leftDistance > rightDistance) {
      // Turn LEFT for 400 ms
      analogWrite(pinLF, 0);
      analogWrite(pinLB, speedSetting);
      analogWrite(pinRF, speedSetting);
      analogWrite(pinRB, 0);
      delay(400); 
    } else {
      // Turn RIGHT for 400 ms
      analogWrite(pinLF, speedSetting);
      analogWrite(pinLB, 0);
      analogWrite(pinRF, 0);
      analogWrite(pinRB, speedSetting);
      delay(400); 
    }
} else {
    // Move forward if no obstacle is detected within 35 cm
    // Left forward motor has +10 here because my left motor is slightly slower than the right one 
    analogWrite(pinLF, speedSetting + 10);
    analogWrite(pinLB, 0);
    analogWrite(pinRF, speedSetting);
    analogWrite(pinRB, 0);
    delay(250); // Drive for 250 ms 
  }
  
}

groundFungus · September 11, 2023, 6:07pm

The usual way to use those drivers is to wire the inx pins to digital pins. Use those pins to control on, off, forward and reverse. Then wire the enx pins to PWM pins and use those to control speed.

kolaha · September 12, 2023, 8:24am

is this a question or not?

stekoel · September 12, 2023, 4:39pm

Thank you for the comment! I have updated the code accordingly.
Note: inx pins are now connected to pins 4-7 on the sensor shield, enx pins to pins 3 and 11.

#include <Servo.h>

Servo myservo;  // Create servo object

int in1 = 4;  // Right motor direction pin 1
int in2 = 5;  // Right motor direction pin 2
int in3 = 6;  // Left motor direction pin 1
int in4 = 7;  // Left motor direction pin 2

int enA = 3;  // Right motor speed (PWM pin)
int enB = 11;  // Left motor speed (PWM pin)

int trigPin = A1;  // Ultrasonic sensor Trigger Pin
int echoPin = A0;  // Ultrasonic sensor Echo Pin

int speedSetting = 140; // Motor speed

void setup() {
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  
  pinMode(trigPin, OUTPUT); // Ultrasonic sensor as OUTPUT
  pinMode(echoPin, INPUT);  // Ultrasonic sensor as INPUT

  myservo.attach(12);  // Attach servo on pin 12
}

int readDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  return duration * 0.034 / 2;
}

void loop() {
  int distance = readDistance();

  if (distance < 40) {
    // Stop
    digitalWrite(in1, LOW);
    digitalWrite(in2, LOW);
    digitalWrite(in3, LOW);
    digitalWrite(in4, LOW);
    analogWrite(enA, 0);
    analogWrite(enB, 0);

    // Measure to the left
    myservo.write(140);
    delay(500);
    int leftDistance = readDistance();

    // Measure to the right
    myservo.write(40);
    delay(500);
    int rightDistance = readDistance();
    
    // Reset servo to center
    myservo.write(90); 
    
    // if there are less than 30 cm in either direction then reverse for 1.5 second and stop. 
    if (leftDistance < 25 || rightDistance < 25){
      // Move backward
      digitalWrite(in1, LOW);
      digitalWrite(in2, HIGH);
      digitalWrite(in3, LOW);
      digitalWrite(in4, HIGH);
      analogWrite(enA, speedSetting);
      analogWrite(enB, speedSetting);        
      delay(1500); 
      // Stop
      digitalWrite(in1, LOW);
      digitalWrite(in2, LOW);
      digitalWrite(in3, LOW);
      digitalWrite(in4, LOW);
      analogWrite(enA, 0);
      analogWrite(enB, 0);
      delay(200); 
    }
    
    // Measure again to the left
    myservo.write(140);
    delay(500);
    leftDistance = readDistance();

    // Measure again to the right
    myservo.write(40);
    delay(500);
    rightDistance = readDistance();

    // Reset servo to center
    myservo.write(90); 
    
    // Turn in the direction with more space
    if (leftDistance > rightDistance) {
      // Turn LEFT for 400 ms
      digitalWrite(in1, HIGH);
      digitalWrite(in2, LOW);
      digitalWrite(in3, LOW);
      digitalWrite(in4, HIGH);
      analogWrite(enA, speedSetting);
      analogWrite(enB, speedSetting);
      delay(250);
    } else {
      // Turn right for 400 ms
      digitalWrite(in1, LOW);
      digitalWrite(in2, HIGH);
      digitalWrite(in3, HIGH);
      digitalWrite(in4, LOW);
      analogWrite(enA, speedSetting);
      analogWrite(enB, speedSetting);
      delay(250);
    }
  } else {
    // Move forward
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
    analogWrite(enA, speedSetting);
    analogWrite(enB, speedSetting);
  }
  
  delay(250); // General delay

}

groundFungus · September 12, 2023, 4:53pm

Good choice of PWM pins. Just FYI, did you see this in the Servo reference?

On boards other than the Mega, use of the library disables analogWrite() (PWM) functionality on pins 9 and 10, whether or not there is a Servo on those pins.

stekoel · September 12, 2023, 6:23pm

Thank you very much for this information! I already thought that the Sensor Shield was not functioning, but that explains it

system · March 10, 2024, 6:23pm

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.

Topic		Replies	Views
Need help with code for ultrasonic sensor car Programming Questions	6	2686	May 6, 2021
Arduino Ultrasonic sensor car Robotics	6	14275	May 6, 2021
stray, declaration and syntax errors... Programming Questions	5	2600	May 5, 2021
help with code for autonomous vehicle using ultrasonic sensor Robotics	3	1704	May 6, 2021
Writing my first sketch and I need HELP !! Programming Questions	6	764	May 5, 2021

Code for ultrasonic sensor car / robotics car kit

Related Topics