Controlled Robot Car with Ultrasonic Sensors Programming Help

Hi, so I've been working on a 4wd robot car controlled with arduino via wifi. Got it working with just the controls pretty nicely. But I've been struggling to implement the ultrasonic sensor into it.

Below is the code for the arduino I'm using. With that I have an wemos d1 mini esp8266 wifi board communicating between the arduino and phone. Pretty much just acting as wlan ap relaying the commands from the phone to the arduino. So far it is working nicely. When I press a button on the phone the car move/turn until I release the button, then it stops. Just like I wanted it.

But when I try to add the sensors into the code I just can't seem to find a way to make it work like I want. And what I want is to stop the car moving forward/backward if the corresponding sensor detects anything closer than 30cm (might change it closer if it feels too far). And also prevent the car from moving to the direction where it detects a block.

I got it to a part where it prevents the moving to a direction where it detects a block. But having really hard time to get it to also stop when already moving.

Also been playing a lot with multiple if and else if clauses but they seem to keep going even when I release the button on the phone.

I'm not a complete beginner with programming but not too experienced either. Any help would be much appreciated. I've checked endless number of other robot car projects but haven't found any with this functionality.

Ps. My circuitry is pretty basic, arduino connected to the wifi module with rx&tx pins, motor driver with pins 8-13, and ultrasonic sensor on pins 2-5. Future plans include on adding a camera with 2 servos moving the base and including the picture on the same app with the controls. Also maybe some leds to light up the camera image.

const int RightB = 8;
const int RightF = 9;
const int RightPWM = 10;
const int LeftPWM = 11;
const int LeftB = 12;
const int LeftF = 13;
const int TrigBack = 2;
const int EchoBack = 3;
const int TrigFront = 4;
const int EchoFront = 5;
long durationBack;
int distanceBack;
long durationFront;
int distanceFront;
int command;
void setup() {
  pinMode(RightB, OUTPUT);
  pinMode(RightF, OUTPUT);
  pinMode(RightPWM, OUTPUT);
  pinMode(LeftPWM, OUTPUT);
  pinMode(LeftB, OUTPUT);
  pinMode(LeftF, OUTPUT);
  pinMode(TrigBack, OUTPUT);
  pinMode(EchoBack, INPUT);
  pinMode(TrigFront, OUTPUT);
  pinMode(EchoFront, INPUT);
  digitalWrite(RightB, LOW);
  digitalWrite(RightF, LOW);
  analogWrite(RightPWM, 0);
  analogWrite(LeftPWM, 0);
  digitalWrite(LeftB, LOW);
  digitalWrite(LeftF, LOW);
  Serial.begin(115200);
}
void loop() {
  if (Serial.available() > 0) {
  command = Serial.read();
  switch (command) {
  case 'F': {
    forward();break;
  }
  case 'B': {
    backward();break;
  }
  case 'R': {
    right();break;
  }
  case 'L': {
    left();break;
  }
  case 'S': {
    stops();break;
  }
  }
  }
}
int frontsensor() {
  digitalWrite(TrigFront, LOW);
  delayMicroseconds(2);
  digitalWrite(TrigFront, HIGH);
  delayMicroseconds(10);
  digitalWrite(TrigFront, LOW);
  durationFront = pulseIn(EchoFront, HIGH);
  distanceFront= durationFront*0.034/2;
  return distanceFront;
}
int backsensor() {
  digitalWrite(TrigBack, LOW);
  delayMicroseconds(2);
  digitalWrite(TrigBack, HIGH);
  delayMicroseconds(10);
  digitalWrite(TrigBack, LOW);
  durationBack = pulseIn(EchoBack, HIGH);
  distanceBack= durationBack*0.034/2;
  return distanceBack;
}
void forward() {
  digitalWrite(RightB, LOW);
  digitalWrite(RightF, HIGH);
  analogWrite(RightPWM, 170);
  analogWrite(LeftPWM, 170);
  digitalWrite(LeftB, LOW);
  digitalWrite(LeftF, HIGH);
}
void backward() {
  digitalWrite(RightB, HIGH);
  digitalWrite(RightF, LOW);
  analogWrite(RightPWM, 170);
  analogWrite(LeftPWM, 170);
  digitalWrite(LeftB, HIGH);
  digitalWrite(LeftF, LOW);
}
void right() {
  digitalWrite(RightB, HIGH);
  digitalWrite(RightF, LOW);
  analogWrite(RightPWM, 140);
  analogWrite(LeftPWM, 140);
  digitalWrite(LeftB, LOW);
  digitalWrite(LeftF, HIGH);
}
void left() {
  digitalWrite(RightB, LOW);
  digitalWrite(RightF, HIGH);
  analogWrite(RightPWM, 140);
  analogWrite(LeftPWM, 140);
  digitalWrite(LeftB, HIGH);
  digitalWrite(LeftF, LOW);
}
void stops() {
  digitalWrite(RightB, LOW);
  digitalWrite(RightF, LOW);
  analogWrite(RightPWM, 0);
  analogWrite(LeftPWM, 0);
  digitalWrite(LeftB, LOW);
  digitalWrite(LeftF, LOW);
}

What I tried was adding a measurement and if condition inside the switch case and inside the motor function. But is it so that the switch case keeps running a single case as long as the button for it is pressed. And not looping through it again and again?
Also tried with if chain but then it stays inside the first if condition it meets, and never stops.

Maybe I'm missing some part of the logic behind these?

Your sketch does nothing unless there is a character to read.

If there is a character to be read, it reads the character and does nothing unless the character is 'F', 'B', 'R', 'L', or 'S'.

If the letter is one of those, it will start doing the desired action and keep doing the desired action until you give it a different desired action.

If you send 'R' and three seconds later send 'S' it should turn right for three seconds and then stop.

You don't call frontsensor() or backsensor() so they don't do anything.

What do you want to do differently?

As I wrote I’d like it to stop moving forward if the frontsensor detects anything under the specified distance, and stop moving backward if the backsensor detects anything under the specified distance. Even if I keep pressing ‘F’ or ‘B’. And also prevent moving forward if the frontsensor detects anything and stop moving backward if the backsensor detects anything.

Is this possible using the switch case stucture, or do I have to change it to something else?

As I wrote I also tested alot with if - else if structure but it didn’t get me anywhere close.

On a sidenote, am I right the switch case only runs the action under one case once, and keeps doing that until the case is changed? I.e. if it gets ‘F’ command, it runs the forward(); once and keeps doing what it specified until it gets a different command to do something else.

Writing from memory as I’m at work, but think my code with ‘if’ was something like this:

...
if (Serial.available() > 0) {
  command = Serial.read()
  if(command = 'F') {
    distanceFront = frontsensor();
    if(distanceFront < 30) {
      stops();
    }
    else if(distanceFront => 30) {
      forward();
    }
...

And think I tried with some delays in there. But why is it that it never exits the ‘if(command = ‘F’) {’ -loop. It just keeps moving forward even if something else is pressed. My experience of coding suggests it should do whats inside and move on with the code to the next part.

Malagath:
On a side note, am I right the switch case only runs the action under one case once, and keeps doing that until the case is changed? I.e. if it gets ‘F’ command, it runs the forward(); once and keeps doing what it specified until it gets a different command to do something else.

Yes.

Malagath:
As I wrote I’d like it to stop moving forward if the frontsensor detects anything under the specified distance, and stop moving backward if the backsensor detects anything under the specified distance. Even if I keep pressing ‘F’ or ‘B’. And also prevent moving forward if the frontsensor detects anything and stop moving backward if the backsensor detects anything.

Malagath:
Writing from memory as I’m at work, but think my code with ‘if’ was something like this:

...

if (Serial.available() > 0) {
  command = Serial.read()
  if(command = ‘F’) {
    distanceFront = frontsensor();
    if(distanceFront < 30) {
      stops();
    }
    else if(distanceFront => 30) {
      forward();
    }

That will keep it from starting to move forward if the front distance is less than 30 but it won’t keep it from continuing to move forward forever if it receives an ‘F’ when the front distance was greater than 30.

It sounds like you want to check frontDistance every time through loop() if the car is moving forward. To do that you need to remember which action function was the last one called. I think you need a global ‘LastAction’ variable that you can set in each of your action functions. At the top of loop() you should check LastAction and if it is ‘forward’, check the front distance, and if it is less than 30, call stops(). Same for ‘backward’ and back distance. It would be useful to have separate actions or forwardLimitStop() and backwardLimitStop() rather than just using ‘stops()’. That way you can ignore ‘F’ (or ‘B’) commands if the forward (or backward) motion hit a distance limit:

  case 'F':
    if (LastAction != FORWARD_LIMIT_STOP)
        forward();
    break;

  case 'B':
    if (LastAction != BACKWARD_LIMIT_STOP)
        backward();
    break;