Posted 08 August 2020,
Now that I have the two 3-element VL53L0X proximity sensors integrated into Wall-E2, my autonomous wall following robot, I have been running some ‘field’ tests in my ‘sandbox’ test area, as shown in the photo below.
As can be seen from the above video, Wall-E2 ‘ran into’ a problem at the end of the second leg. The problem is caused by the extended time required for finding the parallel orientation and capturing the desired wall offset. During this time, Wall-E2 isn’t checking the front distance for upcoming obstacles, and isn’t checking for the ‘isStuck’ condition. In the function that homes to the IR beam on a charging station have the following guard code:
1 2 |
while (bChgConn == LOW && !bIsStuck && frontdist > avoidancedistCm) { |
that checks for a ‘stuck’ condition or an upcoming obstacle. I could also put this same guard code in the functions that handle parallel orientation and offset acquisition/tracking, but it occurred to me that I might want to try off-loading this responsibility to the newly-added Teensy 3.5 I2C slave that manages the dual 3-element VL53L0X proximity sensors. This Teensy spends 99.9% of its time just waiting for the next distance check interval to appear on the horizon, so it would probably welcome something else to do. I could route the LIDAR-lite front LIDAR data to it as well, which would give it all the distance sensors to play with. It could then do the forward/left/right distance array updates, and calculate the forward distance variance that is the heart of the ‘isStuck()’ function. This is all great, but I’m not sure how all that gets integrated back into the main Mega MCU processing loop.
I currently have a ‘receiveEvent(int numBytes)’ implemented on the Teensy to receive a ‘request type’ value from the Mega. This value determines what dataset (left, right, both, just centers, etc) gets sent back to the Mega at the next ‘requestEvent()’ event (triggered by a ‘Wire.requestFrom()’ call from the Mega). So, I could simply add some more request types to ‘GetRequestedVL53l0xValues(VL53L0X_REQUEST which)’ or better yet, add a new function to the Mega to specifically request things like front distance or front distance variance (or simply have the Mega request that the Teensy report the current value of the ‘bisStuck’ boolean variable.
I could also set up a Mega input as an interrupt pin with a CHANGE condition, and have the Teensy interrupt the Mega whenever the ‘stuck’ condition changed (from ‘not stuck’ to ‘stuck, or vice versa). The Mega’s ISR would simply set the ‘bisStuck’ boolean variable to the state of the interrupt input (HIGH or LOW).
Of course, this all presumes there is some real advantage to moving this functionality to the Teensy. If the Mega isn’t processing-challenged, there is no reason to do all this. As this post shows, the time cost for the incremental variance calculation on the Mega is only about 225 uSec, or less than 0.5% of the current 200 mSec loop time.
And, looking at the timestamps on the last actual ‘sandbox’ run, I see that the timestamps during the wall-tracking portion were pretty constant – between 197 and 202 mSec – not the kind of data one would expect from an MCU struggling to keep up.
10 August 2020 Update:
After realizing that the Mega really wasn’t having much problem keeping up with a 200 mSec loop cycle, I went ahead and added the ‘!bIsStuck’ guard to both the ‘RotateToParallelOrientation()’ and ‘CaptureWallOffset()’ functions, thinking this would solve the problem. What actually happened is that as soon as Wall-E2 started running, it immediately sensed a ‘Stuck’ condition and started backing up – WTF! Some head-scratching and some troubleshooting revealed that the while() loops in which I had put the new guard code were running much faster than the main loop (which runs at 200 mSec intervals via use of a ‘elapsedMillis’ variable). What this meant was that it was calculating the forward distance variance hundreds of times per second rather than five, and the forward distance wasn’t changing nearly fast enough to prevent the variance from quickly winding down to zero – oops! In order to fix this problem I had to install some more ‘elapseMillis’ variables to make sure that CalcDistArrayVariance() was called at the same rate as in the main loop, namely every MIN_PING_INTERVAL_MSEC mSec. This works, but now I not only had more ‘Stuck’ guard code scattered throughout my code, but additional kludge-code needed to make the ‘Stuck’ kludge-code work – YUK!!
After thinking about this a bit more, I realized there was another option I hadn’t considered – a timer interrupt set at a convenient interval (like MIN_PING_INTERVAL_MSEC mSec as I am doing now) that does nothing but calculate front distance variance and set bIsStuck accordingly. I haven’t used any timer interrupts up to this point in my Arduino journey, but this seems like a perfect application.
As I normally do, I grabbed a spare Arduino Mega from my parts box, Googled for some timer interrupt examples, and created a demo program to test my theory. First I just did the normal ‘blink without delay’ demo, copying the ‘Timer1’ portion of the code from this post to a new project, and then modifying the ISR to call my ‘CalcDistArrayVariance()’ function with a constant number for the ‘frontdist’ parameter. The CalcDistArrayVariance() function computes the variance of a 25-element array of distance values, and feeding a constant into the function simulates what would happen if the robot gets stuck at some point.
I set up the program to show how long it takes to calculate the variance each time, and how long it takes for the variance to fall to zero. When I ran the program, I got this output:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 |
Port open Timer Interrupt Example last_incmean = 220.00, last_incvar = 14400.00 Msec uSec Var 199 96 12416.000 399 152 10664.961 599 156 9149.446 798 156 7868.160 998 156 6816.000 1198 152 5984.000 1397 156 5359.352 1597 156 4925.437 1797 156 4661.750 1996 156 4544.000 2196 152 4544.000 2396 156 4629.750 2595 156 4765.437 2795 160 4911.352 2995 160 5024.000 3194 156 5056.000 3394 156 4956.160 3594 156 4669.445 3793 156 4136.961 3993 160 3296.000 4193 156 2351.359 4392 156 1552.637 4592 160 898.559 4792 160 384.000 4993 152 0.000 |
As can be seen from the above, calls to CalcDistArrayVariance() occur at 200 mSec intervals and each call takes about 150 uSec (insignificant compared to the 200 mSec loop interval), and it takes about 5 seconds for a constant distance input to produce zero variance on the output. This is pretty much perfect for my application. Before implementing the timer idea, I had over 20 calls to IsStuck() scattered throughout my code, and now they can all be replaced by the boolean bIsStuck variable, which is managed by the timer1 ISR.
Here’s the entire timer interrupt demo code:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 |
/* Name: TimerInterruptDemo.ino Created: 8/10/2020 10:40:48 AM Author: FRANKNEWXPS15\Frank */ #include <PrintEx.h> //allows printf-style printout syntax StreamEx mySerial = Serial; //added 03/18/18 for printf-style printing //storage variables boolean toggle1 = 0; const int MAX_FRONT_DISTANCE_CM = 400; const int FRONT_DIST_ARRAY_SIZE = 25; //04/28/19 - final value (I hope) uint16_t aFrontDist[FRONT_DIST_ARRAY_SIZE]; //04/18/19 rev to use uint16_t vs byte //11/03/18 added for new incremental variance calc double last_incvar = 0; double last_incmean = 0; void setup() { Serial.begin(115200); mySerial.printf("Timer Interrupt Example\n"); cli();//stop interrupts //set timer1 interrupt at 1Hz TCCR1A = 0;// set entire TCCR1A register to 0 TCCR1B = 0;// same for TCCR1B TCNT1 = 0;//initialize counter value to 0 // set compare match register for 1hz increments //OCR1A = 15624;// = (16*10^6) / (1*1024) - 1 (must be <65536) OCR1A = 3124;// = (16*10^6) / (5*1024) - 1 (must be <65536) // turn on CTC mode TCCR1B |= (1 << WGM12); // Set CS10 and CS12 bits for 1024 prescaler TCCR1B |= (1 << CS12) | (1 << CS10); // enable timer compare interrupt TIMSK1 |= (1 << OCIE1A); sei();//allow interrupts pinMode(LED_BUILTIN, OUTPUT); //04/01/15 initialize 'stuck detection' arrays //06/17/20 re-wrote for better readability //to ensure var > STUCK_FRONT_VARIANCE_THRESHOLD for first FRONT_DIST_ARRAY_SIZE loops //array is initialized with sawtooth from 0 to MAX_FRONT_DISTANCE_CM int newval = 0; int bumpval = 20; bool bgoingUp = true; for (int i = 0; i < FRONT_DIST_ARRAY_SIZE; i++) { aFrontDist[i] = newval; //DEBUG!! //mySerial.printf("i = %d, newval = %d, aFrontdist[%d] = %d\n", i, newval, i, aFrontDist[i]); //DEBUG!! if (bgoingUp) { if (newval < MAX_FRONT_DISTANCE_CM - bumpval) //don't want newval > MAX_FRONT_DISTANCE_CM { newval += bumpval; } else { bgoingUp = false; } } else { if (newval > bumpval) //don't want newval < 0 { newval -= bumpval; } else { bgoingUp = true; } } } //04/19/19 init last_incmean & last_incvar to mean/var respectively long sum = 0; for (int i = 0; i < FRONT_DIST_ARRAY_SIZE; i++) { sum += aFrontDist[i]; //adds in rest of values } last_incmean = (float)sum / (float)FRONT_DIST_ARRAY_SIZE; // Step2: calc new 'brute force' variance float sumsquares = 0; for (int i = 0; i < FRONT_DIST_ARRAY_SIZE; i++) { sumsquares += (aFrontDist[i] - last_incmean) * (aFrontDist[i] - last_incmean); } last_incvar = sumsquares / FRONT_DIST_ARRAY_SIZE; mySerial.printf("last_incmean = %3.2f, last_incvar = %3.2f\n", last_incmean, last_incvar); } ISR(TIMER1_COMPA_vect) //timer1 interrupt 1Hz toggles pin 13 (LED) { uint32_t now = micros(); float frontvar = CalcDistArrayVariance(200, aFrontDist); mySerial.printf("%lu\t%lu\t%2.3f\n", millis(), micros() - now, frontvar); //generates pulse wave of frequency 1Hz/2 = 0.5kHz (takes two cycles for full wave- toggle high then toggle low) if (toggle1) { digitalWrite(13, HIGH); toggle1 = 0; } else { digitalWrite(13, LOW); toggle1 = 1; } } void loop() { } double CalcDistArrayVariance(unsigned long newdistval, uint16_t* aDistArray) { //Purpose: Calculate Variance of input array //Inputs: aDistArray = FRONT_DIST_ARRAY_SIZE array of integers representing left/right/front distances //Outputs: Variance of selected array //Plan: // Step1: Calculate mean for array // Step2: Sum up squared deviation of each array item from mean // Step3: Divide squared deviation sum by number of array elements //Notes: // 11/01/18 this function takes about 1.8mSec - small compared to 200mSec loop interval // 11/02/18 added distval to sig to facilitate incremental calc algorithm // 11/12/18 re-wrote incr alg // see C:\Users\Frank\Documents\Arduino\FourWD_WallE2_V1\Variance.xlsm // and C:\Users\Frank\Documents\Arduino\VarianceCalcTest.ino // 01/16/19 added 'return inc_var' // 04/21/19 copied number overflow corrections from VarianceCalcTest.ino // 04/28/19 commented out the 'brute force' sections - now using incr var exclusively //unsigned long funcStartMicrosec = micros(); //11/03/18 update distance array, saving oldest for later use in incremental calcs unsigned long oldestDistVal = aFrontDist[0]; for (int i = 0; i < FRONT_DIST_ARRAY_SIZE - 1; i++) { aFrontDist[i] = aFrontDist[i + 1]; } aFrontDist[FRONT_DIST_ARRAY_SIZE - 1] = newdistval; //11/02/18 now re-do the calculation using the incremental method, and compare the times //mu_t = mu_(t-1) - dist_(t-N)/N + dist_t/N //Example: mu_7 = mu_(6) - dist_(2)/N + dist_7/N //var^2_t = var^2_(t-1) + dist^2_(t) - dist^2_(t-N) + mu^2_(t-1) - mu^2_t //Example: var^2_7 = var^2_(6) + dist^2_(7) - dist^2_(t-N) + mu^2_(6) - mu^2_7 //DEBUG!! //for (int i = 0; i < FRONT_DIST_ARRAY_SIZE; i++) //{ // Serial.print("aDistArray["); Serial.print(i); Serial.print("] = "); Serial.println(aDistArray[i]); //} //DEBUG!! double inc_mean = last_incmean - (double)oldestDistVal / (double)FRONT_DIST_ARRAY_SIZE + (double)newdistval / (double)FRONT_DIST_ARRAY_SIZE; unsigned long olddist_squared = oldestDistVal * oldestDistVal; unsigned long newdist_squared = newdistval * newdistval; double last_incmean_squared = last_incmean * last_incmean; double inc_mean_squared = inc_mean * inc_mean; double inc_var = last_incvar + ((double)newdist_squared / FRONT_DIST_ARRAY_SIZE) - ((double)olddist_squared / FRONT_DIST_ARRAY_SIZE) + last_incmean_squared - inc_mean_squared; //long uSecI = micros() - funcStartMicrosec - uSecB; //DEBUG!! //display results: //mySerial.printf("%lu\t%lu\t%4.2f\t%4.2f\t%4.2f\t%4.2f\t%lu\t%lu\t%4.2f\t%4.2f\t%4.2f\t%4.2f\t%lu\t%lu\n", // newdistval, oldestDistVal, brute_mean, brute_var, last_incmean, last_incvar, olddist_squared, // newdist_squared, inc_mean, inc_mean_squared, last_incmean_squared, inc_var, uSecB, uSecI); //mySerial.printf("%lu\t%lu\t%lu\t%4.2f\t%4.2f\t%4.2f\n", millis(), // newdistval, oldestDistVal, last_incmean, last_incvar, inc_var); //DEBUG!! last_incvar = inc_var; //save for next time last_incmean = inc_mean; //save for next time return inc_var; //added 01/16/19 } |
Stay tuned,
Frank
Pingback: Wall-E3 Update | Paynter's Palace