Loops with conditions

rudolfje · July 8, 2024, 1:01pm

(first code ever, so no idea what i am doing)

I am trying to program a simple line following robot, which has to do a bunch of stuff, but the code my question is about is just following the line and stopping if the SONAR detects something within 10 cm. I am using 3 IR-sensors, and the line is broad enough to be noticed by 2 or even 3.

At first i coded just: if the line is to the right, go to the right, the further away the sharper the turn, but this made my robot be perpendicular to the course half of the times, so i wanted to change something.

My idea was to use a loop which only checked the SONAR and if the line was far right, far left or in the middle.

Lets say the line is to the left, it should then turn to the left, and when the middle IR-sensor noticed the line, it should straighten out a bit to the right (using an if-statement in the void). In theory this should keep the robot probably to one side of the line if you make the straightening out part a bit harsh. But if done correctly, it could straighten out and then the3 IR sensors should all detect the line, and the robot would go straight.

That was the idea. Turns out, if the condition which is set for a void is not true anymore, it doesn't stay in the void, which is what i wanted. I only wanted it to go out if the robot should go straight or to the other side.

I don't even know if this is somewhat possible and how. Any help would be greatly appreciated.

This is my code, which isn't working, and which makes the motors do funny noises as well.

#include <Servo.h>

Servo myservo;
int pos = 0;

// Define pins for infrared sensors
const int infraredSensor1 = 8;
const int infraredSensor2 = 9;
const int infraredSensor3 = 10;

// Define pins for motors
const int motor1Pin = 5;  // Motor 1
const int motor2Pin = 6;  // Motor 2

// Define pins for Sonar
const int trigPin = 2;   // Trigger
const int echoPin = 3;  // Echo

//define variables SONAR
long duration;
int distance;

void setup() {
  // Initialize infrared sensor pins as INPUT
  pinMode(infraredSensor1, INPUT);
  pinMode(infraredSensor2, INPUT);
  pinMode(infraredSensor3, INPUT);

  // Initialize motor pins as OUTPUT
  pinMode(motor1Pin, OUTPUT);
  pinMode(motor2Pin, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(7, OUTPUT);

  // Initialize Sonar sensor pins as INPUT & OUTPUT
  pinMode(echoPin, INPUT);
  pinMode(trigPin, OUTPUT);

  myservo.attach(11);
  myservo.write(0);


  Serial.begin(9600);  
}

int getDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  return duration * 0.034 / 2;
}


void loop() {
  distance = getDistance();


  // Check if distance is less than 10 cm
  if (distance < 10) {
    // Stop both motors
    stopMotors();

  } else {

    if (digitalRead(infraredSensor1) == LOW && digitalRead(infraredSensor2) == HIGH && digitalRead(infraredSensor3) == HIGH) {
      left();
    }

    if (digitalRead(infraredSensor1) == HIGH && digitalRead(infraredSensor2) == HIGH && digitalRead(infraredSensor3) == LOW) {
      right();
    }

    if (digitalRead(infraredSensor1) == LOW && digitalRead(infraredSensor2) == LOW && digitalRead(infraredSensor3) == LOW) {
      straight();
    }
  }
}

void stopMotors() {
  
  digitalWrite(7, LOW);
  analogWrite(motor1Pin, 0);

  digitalWrite(4, LOW);
  analogWrite(motor2Pin, 0);
}

void straight() {
  
  digitalWrite(7, LOW);
  analogWrite(motor1Pin, 180);

  digitalWrite(4, LOW);
  analogWrite(motor2Pin, 180);
}

void left() {
  
    digitalWrite(7, LOW);
    analogWrite(motor1Pin, 220);

    digitalWrite(4, LOW);
    analogWrite(motor2Pin, 120);

    delay(1000); 

    
      if (digitalRead(infraredSensor1) == LOW && digitalRead(infraredSensor2) == LOW && digitalRead(infraredSensor3) == HIGH) {
   
        digitalWrite(7, LOW);
        analogWrite(motor1Pin, 160);

        digitalWrite(4, LOW);
        analogWrite(motor2Pin, 220);

    }
}


void right()  {
    
    digitalWrite(7, LOW);
    analogWrite(motor1Pin, 120);

    digitalWrite(4, LOW);
    analogWrite(motor2Pin, 220);

    delay(1000);


      if (digitalRead(infraredSensor1) == LOW && digitalRead(infraredSensor2) == LOW && digitalRead(infraredSensor3) == HIGH) {
   
        digitalWrite(7, LOW);
        analogWrite(motor1Pin, 220);

        digitalWrite(4, LOW);
        analogWrite(motor2Pin, 160);

    }

}

J-M-L · July 8, 2024, 1:13pm

do you mean "it does not stay in the function" ? (void is a keyword meaning in that context that the function does not return any value).

so do I understand correctly that you would want to stay within this function (for example) while some condition is true ?

void right()  {
    digitalWrite(7, LOW);
    analogWrite(motor1Pin, 120);

    digitalWrite(4, LOW);
    analogWrite(motor2Pin, 220);

    delay(1000);

      if (digitalRead(infraredSensor1) == LOW && digitalRead(infraredSensor2) == LOW && digitalRead(infraredSensor3) == HIGH) { 
        digitalWrite(7, LOW);
        analogWrite(motor1Pin, 220);
        digitalWrite(4, LOW);
        analogWrite(motor2Pin, 160);
    }
}

➜ don't use if (...) {...} because this executes only once. use a while(...) {...} loop statement if you want to repeat the test

blh64 · July 8, 2024, 1:31pm

Welcome to the forum! +1 on putting your code inside code tags.

As for the code, you description doesn't match the code.
For example, you say you want to turn left when only the left sensor detects the line (assuming Sensor1 == left, sensor2 = center, sensor3 = right. Maybe rename variables?)

    if (digitalRead(infraredSensor1) == LOW && digitalRead(infraredSensor2) == HIGH && digitalRead(infraredSensor3) == HIGH) {
      left();

So far, so good, but now you want to to turn left at a certain rate

void left() {
  
    analogWrite(motor1Pin, 220);
    analogWrite(motor2Pin, 120);

Again, so far so good. But now you want to wait until the middle sensor detects the line and then change the rate of turn. At this point, your code is reading all the sensors but you really only need to worry about the middle sensor and you so do with repeatedly using a while() statement

      while (digitalRead(infraredSensor2) == HIGH) {
        // center sensor has not seen line so keep trying
        delay(10);
      }

      // at this point we have detected the line so straighten out a bit
   
      analogWrite(motor1Pin, 160);
      analogWrite(motor2Pin, 220);
    }

Putting that all together, you get this

void left() {

  digitalWrite(7, LOW);
  analogWrite(motor1Pin, 220);

  digitalWrite(4, LOW);
  analogWrite(motor2Pin, 120);

  while (digitalRead(infraredSensor2) == HIGH) {
    // center sensor has not seen line so keep trying
    delay(10);
  }

  // at this point we have detected the line so straighten out a bit

  analogWrite(motor1Pin, 160);
  analogWrite(motor2Pin, 220);
}

the same applies to your right() function

xfpd · July 8, 2024, 5:23pm

SONAR is a distance measuring module for sensing obstacles (walls). The line-sensing module is and I.R. transceiver module that points downward to "see" contrasting light and dark.

gcjr · July 8, 2024, 7:17pm

not sure it's fair to assume the robot will move straight if both motors are set to the same speed (i.e. pwm).

the other thing is left() and right() need to monitor the sensors and make multiple correction, not just wait 1 second and conditionally set the motor speed

next thing is if the speed of one motor is changed (i.e. slowed) when the robot drifts off line, the correction has probably angled the robot to cross the line. so once the robot is on the line, there needs to be an adjustment to take the angle away. but it's not obvious how

if straight() just sets the motors to their original speeds, the robot will just drift off-line again

so it makes sense that the motor corrections try to achieve two different motor speeds that make the robot go straight.

if the motor is slowed by some amount when one sensor is off-line, then speeding the motor up may just maintain a line where the one sensor is off-line.

so after making a correction in left() there could be a loop the repeatedly checks the sensor every so often, sooner than one second. several things can happen,

the one sensor remains off-line,
the middle sensor goes off-line and a greater speed correction is needed,
the 3rd sensor goes off-line and an even greater correction is needed
the 1st sensors goes back on line.

but the thing to worry about is when some correction maintains the sensor states over some period of time. this means the motor speeds are correct to move the robot straight.

the amount of extra correction needed when a 2nd or 3rd sensor goes off line depends on how long it took for the extra sensor to go off-line, less of a correction the longer it takes

now an intentional adjustment can be made to angle the robot back onto the line. And when all the sensors are back on-line, that adjustment needs to be removed so the motor speeds are back to what they were when the sensor state was constant.

loop() should only have a left() and right() and no straight(). the goal is to determine the motor speed settings that track the line (or curve)

hope this make sense. if it does and you have questions, i can provide more help

LoganTheEngineer7 · July 8, 2024, 8:20pm

whoa, you might want to start simple, like a "Hello world" program.

rudolfje · July 8, 2024, 8:24pm

Great idea, but unfortunately i have to do this for my studies, and i am enjoying it a bit finally

LoganTheEngineer7 · July 8, 2024, 8:26pm

okay, we'll see what we can do, this is why we're here.

rudolfje · July 8, 2024, 8:27pm

I absolutely love this idea, but unfortunately I am not nearly adept enough to be doing that, I think

All in all thanks a lot for the advice

rudolfje · July 8, 2024, 8:33pm

I added this to my code, and also applied it to my right void thingy, but still the motor just stops if the middle and right or left sensor detect a line, probably because there is no action for that in the void loop?

Or maybe i should rethink the whole thing, and go for a different tack. Or just always have the robot steer away from the middle if the middle sensor detects a line, and towards the middle if only the outer sensor detects the line. But that is really ugly, isn't it?

Also,

Thanks! I put the ol' discobot to work today

rudolfje · July 8, 2024, 8:34pm

Yes i was aware, my phrasing is just a bit off, sorry.

I meant something along the lines of "checked the SONAR for an obstacle and check the IR-sensors where the line currently is"

rudolfje · July 8, 2024, 8:38pm

yes, something like that. But the condition is actually more that the other conditions in void loop () are not true.

When it goes "into" the void/ function right(), it should keep doing that until the conditions are met for void(left) or void(straight) (but maybe i have to rethink the straight one)

Thanks!

LoganTheEngineer7 · July 8, 2024, 8:54pm

yeah, not all motors rotate at the exact same speed (at the same voltage), so you might need a while statement to adjust the motor speeds if it steers off the road.

kolaha · July 8, 2024, 9:00pm

#include <Servo.h>

Servo myservo;
int pos = 0;

// Define pins for infrared sensors
const int IRS_left = 8;
const int IRS_mid = 9;
const int IRS_right = 10;

// Define pins for motors
const int motor1Pin = 5;  // Motor 1
const int motor2Pin = 6;  // Motor 2

// Define pins for Sonar
const int trigPin = 2;   // Trigger
const int echoPin = 3;  // Echo

//define variables SONAR
long duration;
int distance;

void setup() {
  // Initialize infrared sensor pins as INPUT
  pinMode(IRS_left, INPUT);
  pinMode(IRS_mid, INPUT);
  pinMode(IRS_right, INPUT);

  // Initialize motor pins as OUTPUT
  pinMode(motor1Pin, OUTPUT);
  pinMode(motor2Pin, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(7, OUTPUT);

  // Initialize Sonar sensor pins as INPUT & OUTPUT
  pinMode(echoPin, INPUT);
  pinMode(trigPin, OUTPUT);

  myservo.attach(11);
  myservo.write(0);

  Serial.begin(9600);
}

int getDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH, 6000);
  return duration * 0.017;
}


void loop() {
  distance = getDistance();

  // Check if distance is less than 10 cm
  if (distance < 10)stopMotors();

  else {
//  byte State=digitalRead(IRS_left)|(digitalRead(IRS_mid)<<1)|(digitalRead(IRS_right)<<2);
    byte State = ~PINB & B111;
    if (State & 1)left();
    else if (State & 4)right();
    else if (State == 2)straight();
  }
}

void stopMotors() {
  digitalWrite(7, LOW);
  digitalWrite(motor1Pin, LOW);
  digitalWrite(4, LOW);
  digitalWrite(motor2Pin, LOW);
}

void straight() {
  digitalWrite(7, LOW);
  analogWrite(motor1Pin, 180);
  digitalWrite(4, LOW);
  analogWrite(motor2Pin, 180);
}

void left() {
  digitalWrite(7, LOW);
  digitalWrite(4, LOW);
  analogWrite(motor2Pin, 120);
  delay(10);
}

void right()  {
  digitalWrite(7, LOW);
  analogWrite(motor1Pin, 120);
  digitalWrite(4, LOW);
  analogWrite(motor2Pin, 220);
}

J-M-L · July 8, 2024, 9:04pm

you might benefit from studying state machines. Here is a small introduction to the topic: Yet another Finite State Machine introduction

LoganTheEngineer7 · July 8, 2024, 9:08pm

~~I see we moved to Direct Port Manipulation, but digitalRead(pin); is simpler.~~

rudolfje · July 9, 2024, 2:42pm

I'm afraid I will be sticking to digitalRead instead of DPM, as i have no clue how to use that, but thanks for the help anyway.

rudolfje · July 9, 2024, 2:45pm

This is actually really helpful. I do think that is exactly what i needed, and it made for some fine reading when i had to go out at 4:20 this night to comfort my baby boy.

I tried my best to make something happen with the statemachine, but now there is an "error compiling for board Arduino Uno"

I don't know what that means as it is quite vague.

This is my code as of yet, far from finished, but alright.

#include <Servo.h>

Servo myservo;
int pos = 0;

// Define pins for infrared sensors
const int IRleft = 8;
const int IRmiddle = 9;
const int IRright = 10;

// Define pins for motors
const int motorR = 5;  // Motor 1
const int motorL = 6;  // Motor 2

// Define pins for Sonar
const int trigPin = 2;   // Trigger
const int echoPin = 3;  // Echo

//define variables SONAR
long duration;
int distance;

void setup() {
  // Initialize infrared sensor pins as INPUT
  pinMode(IRleft, INPUT);
  pinMode(IRmiddle, INPUT);
  pinMode(IRright, INPUT);

  // Initialize motor pins as OUTPUT
  pinMode(motorR, OUTPUT);
  pinMode(motorL, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(7, OUTPUT);

  // Initialize Sonar sensor pins as INPUT & OUTPUT
  pinMode(echoPin, INPUT);
  pinMode(trigPin, OUTPUT);

  myservo.attach(11);
  myservo.write(0);


  Serial.begin(9600);
}

enum {middle, left, right} state;


void straight() {

  digitalWrite(7, LOW);
  analogWrite(motorR, 180);

  digitalWrite(4, LOW);
  analogWrite(motorL, 180);

}

void curveRight() {
  digitalWrite(7, LOW);
  analogWrite(motorR, 120);

  digitalWrite(4, LOW);
  analogWrite(motorL, 180);
}

void curveLeft() {
  digitalWrite(7, LOW);
  analogWrite(motorR, 180);

  digitalWrite(4, LOW);
  analogWrite(motorL, 120);
}

void turnRight() {
  digitalWrite(7, LOW);
  analogWrite(motorR, 250);

  digitalWrite(4, LOW);
  analogWrite(motorL, 120);
}

void turnLeft() {
  digitalWrite(7, LOW);
  analogWrite(motorR, 250);

  digitalWrite(4, LOW);
  analogWrite(motorL, 120);
}

void runStateMachine() {
  switch (state) {
    case middle:
      while (digitalRead(IRleft) == LOW && digitalRead(IRmiddle) == LOW && digitalRead(IRright) == LOW) { //Line is in the middle

        straight();
      }

      while (digitalRead(IRleft) == HIGH && digitalRead(IRmiddle) == LOW && digitalRead(IRright) == LOW) {  //Line is slightly to the right

        curveRight();
      }

      while (digitalRead(IRleft) == LOW && digitalRead(IRmiddle) == LOW && digitalRead(IRright) == HIGH) { //Line is slightly to the left

        curveLeft();
      }


      if (digitalRead(IRleft) == HIGH && digitalRead(IRmiddle) == LOW && digitalRead(IRright) == LOW) {  //Line is far right

        state = right;
      }


      else if (digitalRead(IRleft) == HIGH && digitalRead(IRmiddle) == LOW && digitalRead(IRright) == LOW) {   //Line is far left

        state = left;
      }
    case right:

      turnRight();

      while (digitalRead(IRmiddle) == HIGH) {

        curveLeft();

      }
      if (digitalRead(IRleft) == LOW && digitalRead(IRmiddle) == LOW && digitalRead(IRright) == LOW) {   //Line is far left

        state = middle;

      }

      if (digitalRead(IRleft) == LOW && digitalRead(IRmiddle) == LOW && digitalRead(IRright) == LOW) {

        state = left;

      }

    case left:

      turnLeft();

      while (digitalRead(IRmiddle) == HIGH) {

        curveRight();

      }
      if (digitalRead(IRleft) == LOW && digitalRead(IRmiddle) == LOW && digitalRead(IRright) == LOW) {   //Line is far left

        state = middle;

      }

      if (digitalRead(IRleft) == LOW && digitalRead(IRmiddle) == LOW && digitalRead(IRright) == HIGH ) {

        state = right;

      }
  }
}

gcjr · July 9, 2024, 3:15pm

when correcting (i.e. left(), right()) you need to check if things are getting better or worse

const byte PinSen [] = { 8, 9, 10 };
const int Nsen = sizeof (PinSen);

const byte PinMotL = 5;
const byte PinMotR = 6;

const int PwmNorm  = 180;
const int PwmSlow  = 140;
const int PwmSlow2 = 100;

bool s1, s2, s3;

// -----------------------------------------------------------------------------
void
readSensors ()
{
    s1 = digitalRead (PinSen [0]) == HIGH;
    s2 = digitalRead (PinSen [1]) == HIGH;
    s3 = digitalRead (PinSen [2]) == HIGH;
}

// -------------------------------------
void loop ()
{
    readSensors ();

    if (! s1 &&   s2 &&   s3)
            left ();

    if (  s1 &&   s2 && ! s3)
            right ();
}

// -----------------------------------------------------------------------------
void left ()
{
    analogWrite (PinMotR, PwmSlow);

    while (true) {
        delay (200);
        readSensors ();

        if (! s1 && ! s2 &&   s3)
            analogWrite (PinMotR, PwmSlow2);

        if (  s1 &&   s2 &&   s3)  {
            analogWrite (PinMotR, PwmNorm);
            break;
        }
    }
}

// -------------------------------------
void right ()
{
    analogWrite (PinMotL, PwmSlow);

    while (true) {
        delay (200);
        readSensors ();

        if (  s1 && ! s2 && ! s3)
            analogWrite (PinMotL, PwmSlow2);

        if (  s1 &&   s2 &&   s3)  {
            analogWrite (PinMotL, PwmNorm);
            break;
        }
    }
}

// -----------------------------------------------------------------------------
void setup ()
{
    Serial.begin (9600);

    for (unsigned n = 0; n < Nsen; n++)
        pinMode (PinSen [n], INPUT);

    pinMode (4, OUTPUT);
    pinMode (7, OUTPUT);
    digitalWrite (4, LOW);
    digitalWrite (7, LOW);

    analogWrite (PinMotL, PwmNorm);
    analogWrite (PinMotR, PwmNorm);
}

rudolfje · July 9, 2024, 3:43pm

I am having some trouble understanding the code exactly, but it does compile, which is nice, but it doesn't yet work, and because I don't really understand it, I find it difficult to change it.

Topic		Replies	Views
Going from one set of looped commands to another Programming Questions	8	731	May 5, 2021
Please Help Programming Questions	5	477	May 5, 2021
Line follower robot programming Programming Questions	5	1852	May 5, 2021
Problems with if clauses and loop Programming Questions	9	964	May 8, 2022
Need help in coding? Programming Questions	11	1536	May 5, 2021

Loops with conditions

Related Topics