Need help on explanation please

Using Arduino Programming Questions
1

Hi,
The sketch is a Obstacle_Avoiding_Robot used Ultrasonic Distance Sensor, the section confused me is seems every time call void sensorCycle() after first time, need call oneSensorCycle() for the first sensor? why?
Thanks
Adam

..........
//looping the sensors
void sensorCycle() {
  for (uint8_t i = 0; i < SONAR_NUM; i++) {
    if (millis() >= pingTimer[i]) {
      pingTimer[i] += PING_INTERVAL * SONAR_NUM;
      if (i == 0 && currentSensor == SONAR_NUM - 1) oneSensorCycle();
      sonar[currentSensor].timer_stop();
      currentSensor = i;
      cm[currentSensor] = 0;
      sonar[currentSensor].ping_timer(echoCheck);
    }
  }
}
.............

//Return the last valid value from the sensor.
void oneSensorCycle() {
  leftSensor   = returnLastValidRead(0, cm[0]);
  centerSensor = returnLastValidRead(1, cm[1]);
  rightSensor  = returnLastValidRead(2, cm[2]);
}

CODE:

/* https://www.intorobotics.com/obstacle-avoidance-robot/

*/

/*
  :Version 2.0
  :Author: Dragos Calin
  :Email: dragos@intorobotics.com
  :License: BSD
  :Date: 20/04/2020
  :Last update: dd/mm/YYYY
*/
#include <NewPing.h>
#include <SimpleKalmanFilter.h>

#define SONAR_NUM 3          //The number of sensors. 
#define MAX_DISTANCE 200     //Mad distance to detect obstacles.
#define PING_INTERVAL 33     //Looping the pings after 33 microseconds.

int LOOPING              = 10; //Loop for every 10 milliseconds.
int DECREESE_SPEED_LOOP  = 400;//Give some time to sensors for few more readings.
int MOVE_TO_NEW_POSITION = 500;//Wait for the new position.

unsigned long _timerStart         = 0;
unsigned long _timerStartReady    = 0;
unsigned long _timerStartPosition = 0;

uint8_t MIN_RANGE_OBSTACLE = 5; //Between 0 and 5 cm is the blind zone of the sensor.
uint8_t MAX_RANGE_OBSTACLE = 75; //The maximum range to check if obstacle exists.

uint8_t oldSensorReading[3];    //Store last valid value of the sensors.

uint8_t leftSensor;             //Store the sensor's value.
uint8_t centerSensor;
uint8_t rightSensor;

bool isObstacleLeft;           //If obstacle detected or not.
bool isObstacleCenter;
bool isObstacleRight;

uint8_t maximumSpeed = 255; //PWM value for maximum speed.
uint8_t minSpeed = 100; //PWM value for minimum speed.

unsigned long pingTimer[SONAR_NUM]; // Holds the times when the next ping should happen for each sensor.
unsigned int cm[SONAR_NUM];         // Where the ping distances are stored.
uint8_t currentSensor = 0;          // Keeps track of which sensor is active.

NewPing sonar[SONAR_NUM] = {
  NewPing(2, 4, MAX_DISTANCE), // Trigger pin, echo pin, and max distance to ping.
  NewPing(5, 6, MAX_DISTANCE),
  NewPing(7, 8, MAX_DISTANCE)
};

SimpleKalmanFilter KF_Left(2, 2, 0.01);
SimpleKalmanFilter KF_Center(2, 2, 0.01);
SimpleKalmanFilter KF_Right(2, 2, 0.01);

enum NavigationStates {
  CHECK_ALL,
  MAX_SPEED,
  SPEED_DECREASE,
  CHECK_OBSTACLE_POSITION,
  LEFT,
  CENTER,
  RIGHT,
  BACK
};
NavigationStates _navState = CHECK_ALL;

//L298N motor driver pins
byte enA = 3;
byte in1 = 10;
byte in2 = 11;
byte enB = 9;
byte in3 = 12;
byte in4 = 13;

/*SETUP & LOOP*/
void setup() {
  Serial.begin(115200);

  pingTimer[0] = millis() + 75;
  for (uint8_t i = 1; i < SONAR_NUM; i++)
    pingTimer[i] = pingTimer[i - 1] + PING_INTERVAL;

  pinMode(enA, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);

  stopMotors();
}

void loop() {
  if (isTimeForLoop(LOOPING)) {  // if its time to start?
    sensorCycle();   ///// get ULTs reads
    applyKF();       ///// filter the reading results 
    obstacleAvoidance();
    startTimer();
  }
}

void startTimer() {
  _timerStart = millis();
}

void startTimerReady() {
  _timerStartReady = millis();
}

void startTimerPosition() {
  _timerStartPosition = millis();
}

bool isTimeForLoop(int _mSec) {
  return (millis() - _timerStart) > _mSec;
}

bool isTimerReady(int _mSec) {
  return (millis() - _timerStartReady) > _mSec;
}

bool isTimerPosition(int _mSec) {
  return (millis() - _timerStartPosition) > _mSec;
}

//looping the sensors
void sensorCycle() {
  for (uint8_t i = 0; i < SONAR_NUM; i++) {
    if (millis() >= pingTimer[i]) {
      pingTimer[i] += PING_INTERVAL * SONAR_NUM;
      if (i == 0 && currentSensor == SONAR_NUM - 1) oneSensorCycle();
      sonar[currentSensor].timer_stop();
      currentSensor = i;
      cm[currentSensor] = 0;
      sonar[currentSensor].ping_timer(echoCheck);
    }
  }
}

// If ping received, set the sensor distance to array.
void echoCheck() {
  if (sonar[currentSensor].check_timer())
    cm[currentSensor] = sonar[currentSensor].ping_result / US_ROUNDTRIP_CM;
}

//Return the last valid value from the sensor.
void oneSensorCycle() {
  leftSensor   = returnLastValidRead(0, cm[0]);
  centerSensor = returnLastValidRead(1, cm[1]);
  rightSensor  = returnLastValidRead(2, cm[2]);
}

//If sensor value is 0, then return the last stored value different than 0.
int returnLastValidRead(uint8_t sensorArray, uint8_t cm) {
  if (cm != 0) {
    return oldSensorReading[sensorArray] = cm;
  } else {
    return oldSensorReading[sensorArray];
  }
}

//Apply Kalman Filter to sensor reading.
void applyKF() {
  isObstacleLeft = obstacleDetection(KF_Left.updateEstimate(leftSensor));
  isObstacleCenter = obstacleDetection(KF_Center.updateEstimate(centerSensor));
  isObstacleRight = obstacleDetection(KF_Right.updateEstimate(rightSensor));
}

//Define the minimum and maximum range of the sensors, and return true if an obstacle is in range.
bool obstacleDetection(int sensorRange) {
  if ((MIN_RANGE_OBSTACLE <= sensorRange) && (sensorRange <= MAX_RANGE_OBSTACLE)) return true; else return false;
}

//Obstacle avoidance algorithm.
void obstacleAvoidance()
{
  switch (_navState) {
    case CHECK_ALL: { //if no obstacle, go forward at maximum speed
        if (isObstacleLeft == 0 && isObstacleCenter == 0 && isObstacleRight == 0) {
          _navState = MAX_SPEED;
        } else {
          startTimerReady();
          _navState = SPEED_DECREASE;
        }
      } break;

    case MAX_SPEED: {
        moveForward(maximumSpeed);
        _navState = CHECK_ALL;
      } break;

    case SPEED_DECREASE: {
        moveForward(minSpeed);
        //Wait for few more readings at low speed and then go to check the obstacle position
        if (isTimerReady(DECREESE_SPEED_LOOP)) _navState = CHECK_OBSTACLE_POSITION;
      } break;

    case CHECK_OBSTACLE_POSITION: {
        //If the path is free, go again to MAX_SPEED else check the obstacle position
        if (isObstacleLeft == 0 && isObstacleCenter == 0 && isObstacleRight == 0) {
          _navState = MAX_SPEED;
        }
        else if (isObstacleLeft == 1 && isObstacleCenter == 0 && isObstacleRight == 0) {
          startTimerPosition();
          _navState = LEFT;
        }
        else if (isObstacleLeft == 0 && isObstacleCenter == 1 && isObstacleRight == 0) {
          startTimerPosition();
          _navState = CENTER;
        }
        else if (isObstacleLeft == 0 && isObstacleCenter == 0 && isObstacleRight  == 1) {
          startTimerPosition();
          _navState = RIGHT;
        }
        else if (isObstacleLeft == 1 && isObstacleCenter == 1 && isObstacleRight == 1) {
          startTimerPosition();
          _navState = BACK;
        }
      } break;
  
    case LEFT: { //Move left and check obstacle. If obstacle exists, go again to left, else exit
        moveLeft(minSpeed);
        if (isTimerPosition(MOVE_TO_NEW_POSITION)) {
          if (isObstacleLeft == 1) _navState = LEFT;
          else _navState = CHECK_ALL;
        }
      } break;

    case CENTER: { //If obstacle exists, go left or right
        if (randomMove() == 1)  _navState = LEFT; else  _navState = RIGHT;
      } break;

    case RIGHT: {
        moveRight(minSpeed);
        if (isTimerPosition(MOVE_TO_NEW_POSITION)) {
          if (isObstacleRight == 1) _navState = RIGHT;
          else _navState = CHECK_ALL;
        }
      } break;

    case BACK: {
        moveBackward(minSpeed);
        if (isTimerPosition(MOVE_TO_NEW_POSITION)) {
          if (randomMove() == 1)  _navState = LEFT; else  _navState = RIGHT;
        }
      } break;
  }
}

//L298N Motor Driver.
void stopMotors() {
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);
}

void moveForward(uint8_t pwmValue) {
  digitalWrite(in1, HIGH);
  digitalWrite(in2, LOW);
  digitalWrite(in3, HIGH);
  digitalWrite(in4, LOW);
  analogWrite(enA, pwmValue);
  analogWrite(enB, pwmValue);
}

void moveBackward(uint8_t pwmValue) {
  digitalWrite(in1, LOW);
  digitalWrite(in2, HIGH);
  digitalWrite(in3, LOW);
  digitalWrite(in4, HIGH);
  analogWrite(enA, pwmValue);
  analogWrite(enB, pwmValue);
}

void moveLeft(uint8_t pwmValue) {
  digitalWrite(in1, LOW); //Left wheel backward.
  digitalWrite(in2, HIGH);
  digitalWrite(in3, HIGH);//Right wheel forward.
  digitalWrite(in4, LOW);
  analogWrite(enA, pwmValue);
  analogWrite(enB, pwmValue);
}

void moveRight(uint8_t pwmValue) {
  digitalWrite(in1, HIGH); //Left wheel forward.
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);//Right wheel backward.
  digitalWrite(in4, HIGH);
  analogWrite(enA, pwmValue);
  analogWrite(enB, pwmValue);
}

//Return 1 or 0 from a random number. 
int randomMove() {
  uint8_t rnd_number = random(1, 100);
  return rnd_number % 2;

}
2

IDK, how about you explain how it works? There's a lot of fancy footwork involving millis() with minimal comments.

1 Like
3

sensorCycle() does seem confusing.

it only calls oneSensorCycle() after the first time it is called when currentSensor has been incremented to SONAR_NUM-1.

oneSensorCycle() sets cm[], but subsequent iterations of the loop within sensorCycle resets cm [] to zero

4

Is it OK to use iterations method in multi-sensor? are there any examples please?
Thanks

5

I said, it's extremely confusing code. To get answers, you first have to explain how it works.

If you don't know because you didn't write it, realize that sometimes the reason for excessive complexity or opaqueness, is simply programmer incompetence. Sometimes there is really no more reason than that.

6

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