Techylah:
"Making it [2][37] instead of [37][2] might increase speed (compiler can use smaller increments).
Also don't include a subroutine call in the loop."Could you describe in more detail why these slow it down?
If it's set up the first way, the compiler can keep the address in a CPU register. After writing the data to that address it then increment the register by 1 or 4 (for byte or int arrays). These increment register instructions are typically built in to the instruction set. Doing it the second way, the compiler must insert either an "add 37" instruction or an "Add 148" instruction to get to the next address.
Also the overhead for calling a subroutine, especially if it has arguments, is more than not having a subroutine call at all. Many compilers have a "inline function" system to let you appear to write a function, but have the code for it inserted in your flow as if it weren't a function or subroutine. That takes up more space but is faster.These are really for squeezing the most speed out of a tight routine. In this case it is the printing of needless words and numbers that is taking the vast majority of the time.
Thanks for the explanation. You were right about the extra text bogging it down. Now it only prints out the direction the car would go (represented as a pulse width from 800 us to 1700 us, where 800 us is hard right and 1700 us is hard left). Even this will be unnecessary in the final version of the vehicle. I found the other measures you suggested interesting and I will keep them in mind when looking for ways to optimize code in future projects (maybe this one if I find the sensing is too sluggish when I take the car out for a drive).
Now, the car can choose the shortest of 36 obstacle free paths towards a waypoint in under a second. In this example, the way point is in the direction that corresponds to a pulse width of 1400 us.
boolean debug = false;
// setup ultrasonic distance sensor
const int pingPin = 2;
unsigned int duration, distance;
int sensorWait = 10;
// setup servo under sensor
#include <Servo.h>
Servo sensingServo;
int pos = 1300;
int sensingServoMin = 800;
int sensingServoMax = 1700;
int sensingServoResolution = 25;
boolean sweepLeft = true;
// setup array for storing directions and distances
// array length = ((sensingServoMax - sensingServoMin) / sensingServoResolution) + 1;
int directionsAndDistances[37][2];
int directionsAndDistancesLength = 37;
int i;
// setup arrray for choosing direction
int obstacleFreeDirections[37][3];
int minimumDistance = 48;
int waypointDirection = 1400;
void readSensor()
{
// move servo into position
sensingServo.writeMicroseconds(pos);
// read ultrasonic sensor
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(5);
digitalWrite(pingPin, LOW);
pinMode(pingPin, INPUT);
duration = pulseIn(pingPin, HIGH);
distance = duration / 74 / 2;
// write position and distance to array
directionsAndDistances[i][0] = pos;
directionsAndDistances[i][1] = distance;
}
int chooseDirection()
{
if(debug)
{
Serial.println("Directions and distances");
for (int i = 0; i < directionsAndDistancesLength; i++)
{
Serial.print(directionsAndDistances[i][0]);
Serial.print(",");
Serial.print(directionsAndDistances[i][1]);
Serial.print("\n");
}
}
// filter out directions with obstacles
for(int i,j = 0; i < directionsAndDistancesLength; i++)
{
if (directionsAndDistances[i][1] >= minimumDistance)
{
obstacleFreeDirections[j][0] = directionsAndDistances[i][0];
obstacleFreeDirections[j][1] = directionsAndDistances[i][1];
}
else
{
obstacleFreeDirections[j][0] = 0;
obstacleFreeDirections[j][1] = 0;
}
obstacleFreeDirections[j][2] = 0;
j++;
}
if(debug)
{
Serial.println("Obstacle free directions");
for (int i = 0; i < directionsAndDistancesLength; i++)
{
Serial.print(obstacleFreeDirections[i][0]);
Serial.print(",");
Serial.print(obstacleFreeDirections[i][1]);
Serial.print(",");
Serial.print(obstacleFreeDirections[i][2]);
Serial.print("\n");
}
}
// calculate the discrepancy between the waypoint direction and each direction
for(int i = 0; i < directionsAndDistancesLength; i++)
{
if(obstacleFreeDirections[i][0] != 0)
{
obstacleFreeDirections[i][2] = abs(waypointDirection - obstacleFreeDirections[i][0]);
}
}
if(debug)
{
Serial.println("Discrepancies from way point");
for (int i = 0; i < directionsAndDistancesLength; i++)
{
Serial.print(obstacleFreeDirections[i][0]);
Serial.print(",");
Serial.print(obstacleFreeDirections[i][1]);
Serial.print(",");
Serial.print(obstacleFreeDirections[i][2]);
Serial.print("\n");
}
}
// choose the direction closest to the waypoint direction with no obstacles
// find the first obstacle free direction
int solutionDirectionReference = 0;
while(obstacleFreeDirections[solutionDirectionReference][0] == 0)
{
solutionDirectionReference++;
}
// find the obstacle free direction closest to the waypoint direction
for(int i = 0; i < directionsAndDistancesLength; i++)
{
if(obstacleFreeDirections[i][0] != 0 && obstacleFreeDirections[i][2] < obstacleFreeDirections[solutionDirectionReference][2])
{
solutionDirectionReference = i;
}
}
if(obstacleFreeDirections[solutionDirectionReference][0] > 15000)
{
return 0;
}
else
{
return obstacleFreeDirections[solutionDirectionReference][0];
}
}
void setup()
{
Serial.begin(115200);
sensingServo.attach(5);
}
void loop() {
if (sweepLeft)
{
// sweep left
i = 0;
for(pos = sensingServoMin; pos <= sensingServoMax; pos += sensingServoResolution)
{
readSensor();
i++;
delay(sensorWait);
}
sweepLeft = false;
}
else
{
// sweep right
i = directionsAndDistancesLength - 1;
for(pos = sensingServoMax; pos >= sensingServoMin; pos -= sensingServoResolution)
{
readSensor();
i--;
delay(sensorWait);
}
sweepLeft = true;
}
int chosenDirection = chooseDirection();
// Serial.print("Direction chosen: ");
Serial.print(chosenDirection);
Serial.print("\n");
}