Code for driving motor based on distance sensor values

Hi all,
I am currently working on a project that uses 4 distance sensors (2 on the back and 2 on the left) to work out my position on a track, and then instruct the vehicle to go straight/turn etc. I was just wondering what is the best way to do the code for this? I was thinking of making a subroutine for each of the motor functions (turn left, right, stop etc) and another for the function of the sensors (read value, convert to distance etc.). But I'm struggling to think of a way to code it all because if I just use the one void loop, I would have to try and do if statements for every possible situation the vehicle could be in along the course (since it has to loop every fraction of a second to read the sensor values). I'm sure there's a very simple solution but I'm struggling to find it.

Thanks in advance for any help and let me know if you would like any additional info :slight_smile:

I was thinking of making a subroutine for each of the motor functions (turn left, right, stop etc) and another for the function of the sensors (read value, convert to distance etc.).

Sounds like a reasonable approach.

But I’m struggling to think of a way to code it all because if I just use the one void loop, I would have to try and do if statements for every possible situation the vehicle could be in along the course

How many possible situations are there? You are moving forward. There is, or is not, something in front of you. If there is something, it may require a left turn or a right turn. So, that’s 4. And there are 4 similar ones for when going backwards. That doesn’t seem like an insurmountable number of situations.

I’ve attached a picture of the track and the vehicle (the arrows indicate sensors). Basically the track is all flat with no walls except the low wall that I’ve labelled and the 2 steps. Basically the commands we want to use are:

  1. drive until the back sensors read a certain distance (eg. 5cm) and then stop {this is to detect the first step}
  2. realign the vehicle using the distance values of the 2 back sensors.
  3. Continue driving until the back sensors read the distance again (eg. 5cm), {this will occur after the vehicle has climbed the second step.}
  4. Rotate 90 degrees counter-clockwise until the value of the 2 left sensors are equal
  5. drive a certain distance to the finish line then stop.

So my main problem is working out how to differentiate the first time the back sensors read 5cm and the second time, as these 2 situations require different things from the motor.
Also thanks for all your advice, you’ve been very helpful so far!!

I would approach it this way.

In loop()
call function(s) to read the distances from the sensors.
call function(s) to command motors based on the readings from the sensors.

I use the term function(s), because it’s really up to you as to whether to use one function to read all the sensors, or multiple functions, each one reading one or more sensors.

Similarly, you can call a function to decide whether or not you need to perform a motor change, and from that function, call the appropriate function to actually change the motor(s) speed.

Ok thanks! I'll give that a try and see how I go. But just a quick question, can i use void for my functions involving sensors even though they are returning a value? If not how do I start those functions?

can i use void for my functions involving sensors even though they are returning a value?

No. The return type and the returned value must be compatible.

If not how do I start those functions?

How do you "start" digitalRead()?

If its not too much to ask would you be able to just have a look at my code? Because I had it checked in another unrelated post and they said the syntax seemed fine so I just want to double check there’s definitely an error before I change it.
Thanks a lot for your help! :slight_smile:

#define ds_pinb1 0    //s   // Pin to which the distance sensor is connected
#define ds_pinb2 1    //s      //b1 refers to 1st sensor on back, b2 is second sensor on back, s1 refers to 1st pin on left side etc. Pins are counted in a clockwise direction.
#define ds_pins1 2    //s
#define ds_pins2 3    //s
#define buttonPin 4   //b



int motor_left[] = {2, 3};      //m      ////////CHECK THESE
int motor_right[] = {7, 8};     //m      ////////NUMBERS//////these are arrays, so motor_right[0]=7 and motor_right[1]=8 etc.

int ds_valueb1;      //s      // Integer to hold the sensor’s reading
int ds_valueb2;      //s
int ds_values1;      //s
int ds_values2;      //s

float ds_scaledValueb1;    //s // Distance sensor’s scaled value
float ds_scaledValueb2;    //s
float ds_scaledValues1;    //s
float ds_scaledValues2;    //s

void setup()
{
  Serial.begin (9600); // Initiates serial communication at 9600 baud
  
 
  
int i;                            //m
for(i = 0; i < 2; i++){           //m 
pinMode(motor_left[i], OUTPUT);   //m
pinMode(motor_right[i], OUTPUT);  //m
}
}
void loop (){

  sensor();
  sum_value();
  int sum_sensor;
  drive_forward();
    
if (ds_scaledValueb1 ==50 || ds_scaledValueb2 == 50)    //50 is in mm, may have to change calibration to get this value
  {
  motor_stop();
  if (sum_sensor<100){      //this vale of 100 has to be determined, it will just be a value greater than the total distance the sensor has read as the vehicle stops this first step (just to differentiate between first and second step)
    while (ds_scaledValueb1 != ds_scaledValueb2){
      if (ds_scaledValueb1 > ds_scaledValueb2)
        turn_right();
      else if (ds_scaledValueb1 < ds_scaledValueb2)
        turn_left();}
      drive_forward();
      delay(200);}
  
  else if (sum_sensor>100){
    while (ds_scaledValues1 != ds_scaledValues2){
      if (ds_scaledValues2 > ds_scaledValues1){
        turn_left();}
      else if (ds_scaledValues1 > ds_scaledValues2)    //this part is just in case the motor turns too far
        turn_right();}
    drive_forward();}
    delay(10000);    //time delay has to be determined, based on distance the vehicle needs to travel to finish in end zone.
    motor_stop();}}
       

  // --------------------------------------------------------------------------- Drive
void sum_value(){
  int sum_sensor;
  sum_sensor=sum_sensor+(ds_scaledValueb1+ds_scaledValueb2)/2;
}

void sensor(){
  ds_valueb1 = analogRead (ds_pinb1);    //s               // Reads the distance sensor
  ds_valueb2 = analogRead (ds_pinb2);    //s
  ds_values1 = analogRead (ds_pins1);    //s
  ds_values2 = analogRead (ds_pins2);    //s
  
  ds_scaledValueb1 = ((float)ds_valueb1 / 615) * 1024;    //s                 // Calculates the scaled value
  ds_scaledValueb2 = ((float)ds_valueb2 / 615) * 1024;    //s
  ds_scaledValues1 = ((float)ds_values1 / 615) * 1024;    //s
  ds_scaledValues2 = ((float)ds_values2 / 615) * 1024;    //s
}

void motor_stop(){
digitalWrite(motor_left[0], LOW); 
digitalWrite(motor_left[1], LOW); 

digitalWrite(motor_right[0], LOW); 
digitalWrite(motor_right[1], LOW);
delay(25);
}

void drive_forward(){
digitalWrite(motor_left[0], HIGH); 
digitalWrite(motor_left[1], LOW); 

digitalWrite(motor_right[0], HIGH); 
digitalWrite(motor_right[1], LOW); 
}

void drive_backward(){
digitalWrite(motor_left[0], LOW); 
digitalWrite(motor_left[1], HIGH); 

digitalWrite(motor_right[0], LOW); 
digitalWrite(motor_right[1], HIGH); 
}

void turn_left(){
digitalWrite(motor_left[0], LOW); 
digitalWrite(motor_left[1], HIGH); 

digitalWrite(motor_right[0], HIGH); 
digitalWrite(motor_right[1], LOW);
}

void turn_right(){
digitalWrite(motor_left[0], HIGH); 
digitalWrite(motor_left[1], LOW); 

digitalWrite(motor_right[0], LOW); 
digitalWrite(motor_right[1], HIGH); 
}

Please try to use the auto format tool in the IDE before posting code - your indentation makes my eyes ache.

Sorry would you like me to repost?

int ds_valueb1;      //s      // Integer to hold the sensor’s reading
int ds_valueb2;      //s
int ds_values1;      //s
int ds_values2;      //s

Why do these need to be global?

float ds_scaledValueb1;    //s // Distance sensor’s scaled value
float ds_scaledValueb2;    //s
float ds_scaledValues1;    //s
float ds_scaledValues2;    //s

Or these?

  sensor();

Rubbish name. A good function name includes a noun AND a verb, like digitalRead(). This name does not at all describe what the function does. In fact, the function needs to be completely rewritten, to be passed a pin number and to return a single value. It then needs to be called 4 times, for each sensor it is supposed to read. And, there should be NO interaction with global variables.

void sum_value(){
  int sum_sensor;
  sum_sensor=sum_sensor+(ds_scaledValueb1+ds_scaledValueb2)/2;
}

How does this accomplish anything? The local variable that is written to goes out of scope immediately after it gets a value.

        turn_right();}
    drive_forward();}

} do NOT go on the same line with any other code, no matter what style you are following.

    motor_stop();}}

And there should NEVER be more than one } on any line.

Because I had it checked in another unrelated post and they said the syntax seemed fine so I just want to double check there's definitely an error before I change it.

You should change it because it is not the best code possible, regardless of whether there is an error, or not. What error do you suspect? Have you actually tried executing the code?

Why are you asking the same questions in two different parts of the forum ?