Readings values sensors take (too) long

Hello my friends,

I’m busy with building a micromouse (a small robot which should be able to solve a “random” maze).
For this I’m using an Adafruit Itsybitsy M4 microcontroller and time to flight sensors (on each side and two at the front of my robot).

Right now I’m facing a quite annoying problem… the readings of the sensors take relatively quite a long time… And this makes that my robot is overshooting and my loop is being delayed by readings these values. In my code below I’m trying to let my micromouse accelerate for a certain amount of counts (counted by my encoders), and if possible I want that my robot during this acceleration is going to follow the wal.

	while ((s-ds) > 150) {
		// Readings both left and right sensor (VL53L0X time to flight sensors)
		leftSensorReadings = Sensor::readSensorLeft();
		rightSensorReadings = Sensor::readSensorRight();

		unsigned long currentMillis = millis();

		if (leftSensorReadings < 100 & rightSensorReadings < 100) { //Whenever both left and right wall are true, go and do wall following
		else { 	// Else runMotorsPID (runMotors with encoder pid)
			runMotorsPID(0, speed);

		if (currentMillis - previousMillis > interval1) {	// acceleration every 8ms
			previousMillis = currentMillis;
			speed = speed + 3.5;

		ds = (distanceLeftCount + distanceRightCount) / 2; //Noting down the current count of the encoders

In the code below the current values of the sensors are being read:

float Sensor::readSensorLeft() {
	leftSensor = sensor2.readRangeSingleMillimeters();
	return leftSensor;

float Sensor::readSensorRight() {
	rightSensor = sensor3.readRangeSingleMillimeters();
	return rightSensor;

float Sensor::readSensorFront() {
	frontSensor = (sensor1.readRangeContinuousMillimeters() + sensor4.readRangeContinuousMillimeters()) / 2;
	return frontSensor;

So in short, the problem is that whenver I’m reading the values of my sensors the code is being “paused” for a second which causes that everything happens a fraction of time later, now my question is, is there a better way of tackling these sensor readings in a shorter amount of time?

Why not

float Sensor::readSensorLeft() 
  return  sensor2.readRangeSingleMillimeters();

? (Etc)

Just asking…
Where is your code?

Well I could post the remaining 2000 lines of code over here, but I do believe that the posted code should be enough for this topic.

You are perfectly entitled to your beliefs, however irrational.

Good luck.

Thank you for the amazing help, respect...

Thank you for the amazing help, respect…

You’re very welcome.

Anytime you need help, just post the relevant code and observations.

The rest of the code is mostly used for floodfill algorithm, which is an algorithm in which the micromouse searches the shortest path in the maze.

Interesting, but irrelevant (and another five minutes wasted). (sp. “path”)

In the attachment all my code is included, since it’s simply to long to post over here.

I know it’s quite some code, however let me describe my problem as I already did in the first post. I am having a hard time with the readings of the sensors, since it costs a lot of time… For example when I’m wall following and this is my psuedocode:

while (both walls are true) {

In this problem for example the reading of the sensors takes up quite some time, and therefore the program still runs a couple of ms even one of the wall aren’t true anymore. This causes my micromouse to overshoot. So I am looking for a solution in which the reading of the sensors will take up less time, or maybe another tactic which could work…

const.h (1.48 KB)

Floodfill.ino (4.46 KB)

Maze.cpp (11.5 KB)

Maze.h (1.66 KB)

Movement.cpp (12.7 KB)

Movement.h (745 Bytes)

Sensor.cpp (4.04 KB)

Sensor.h (402 Bytes)

I am having a hard time with the readings of the sensors, since it costs a lot of time

I’d be asking myself “Why is that?” and, very shortly afterwards “How can I investigate that, independent of all the other code?”

I'd be asking myself "Why is that?" and, very shortly afterwards "How can I investigate that, independent of all the other code?"

I do believe that it is because the function digitalRead simply costs time. The best would be that I would only have to check the sensor reading if they change, however you can't know wether they've changed without reading them first...
The best solution would be to read the sensors continously while the normal code is still running and just take the most up to date values whenever I need them, I do fear though that this is only possible with a second arduino (which in my case is not a solution)...

I do believe that it is because the function digitalRead simply costs time.

Well, yes, it takes time.
Which particular digitalRead do you consider to be problematic? (Bearing in mind you appear to be using an array of I2C sensors)

How much time do you think it takes?
You don’t think the delays take more time?

Well I believe, especially looking at the millis() and watching the difference without the sensorreadings ans with that whenever I call the function Sensor::readSensorLeft/Right which is in fact the same as sensor2/3.ReadRangeSingleMillimeters() takes about 4-5ms

Could you please specify which delays you mean?

Please rest assured that a digitalRead is probably about 1000 times faster than 4-5ms.

You’re looking in the wrong place.