The wander circuit is implemented below in pseudocode.
All variables/constants doubles unless otherwise specified
Constants:
gain = 0.25
decay = 0.1
threshold = 0.5;
External variables:
baseSpeed
goRight
goLeft
goStraight
rightMotor
leftMotor
Internal variables:
rightOscillator
rightTrigger {
input
output
}
leftOscillator
leftTrigger {
input
output
}
Initialize:
rightTrigger.input = 0;
rightTrigger.output = 0;
leftTrigger.input = 0;
leftTrigger.output = 0;
Utilities:
double bound(double v)
{
double ret = 0.0;
if (v > 0.99) ret = 0.99;
else if (v < 0.0) ret = 0.01;
return ret;
}
double random(double max)
{
generate a random double in the range 0.0 .. max
}
Execute every 1/10 second:
rightOscillator = bound(0.5 + random(0.1) + leftTrigger.output
-
rightTrigger.output - goRight - goStraight);
leftOscillator = bound(0.5 + random(0.1) + rightTrigger.output
-
leftTrigger.output - goLeft - goStraight);
rightTrigger.input = bound(rightTrigger.input +
rightOscillator * gain -
rightTrigger.input * decay);
if (rightTrigger.input > threshold) {
rightTrigger.output = bound(rightTrigger.input + random(0.1));
rightTrigger.input = random(0.1);
}
leftTrigger.input = bound(leftTrigger.input +
leftOscillator * gain -
leftTrigger.input * decay);
if (leftTrigger.input > threshold) {
leftTrigger.output = bound(leftTrigger.input + random(0.1));
leftTrigger.input = random(0.1);
}
End Execute
Execute every 1/10 second:
rightMotor = bound(baseSpeed + rightOscillator);
leftMotor = bound(baseSpeed + leftOscillatory);
End Execute