A thread for Algorithms

VEX V5 General Discussion General Forum
5

Oooh, this looks like it could be a fun thread!

Hey, I recognize that username… :smiley:

On a serious note, while (as I mentioned in the position tracking document) we don’t exactly want to just release the math/code for the algorithms we used last year, as we’d much rather teach people how we came up with those algorithms as opposed to just handing over a “working” solution that would not at all be optimized to everyone’s needs. Part of the reason we haven’t released anything beyond the initial document is a combination of not having the time to write it, and that explaining that kind of thing in a written document only is quite difficult (I’m aware that the tracking doc isn’t exactly the easiest thing to follow, has some typos, etc…)

@Andrew_Strauss and I have discussed the possibility of starting a YouTube video series on robotics programming, so maybe that could make its way there… but no promises, given that we’re already stretched on time and we’re not even competing in VEX U yet.

Nonetheless, I’ll try to go over a couple simple algorithms that we used last year:

Nearest equivalent angle: Simple mathematical trick, very useful as a building block in building motion algorithms on top of a position tracking system. Anyway, assuming that you’re trying to find the equivalent angle for \theta_{target} that’s closest to \theta_{ref}, both in radians:

\left[ \frac {\theta_{ref}-\theta_{target}} {2\pi} \right] \times 2\pi + \theta_{target}

Where, for lack of better notation, \left[ x \right] denotes the nearest integer to some real number x (we just used ROBOTC’s round function.

How does this work? Well, the rounding part calculates how many rotations off you are from the target (the expression inside is the difference between the target and reference angle values, measured in rotations rather than radians - the conversion is done by dividing by 2\pi). The outside then converts that offset in integer revolutions back to radians, which gives how much you need to change the target by to get to the nearest equivalent angle… and then adds that to the target, thus calculating the result.

Ultrasonic filtering: Basically what it says. We used this to ensure that all of our ultrasonic values were actually picking up on the intended object.

  1. Set a target number of “good” readings (for example, 10)
  2. Set a maximum number of readings to take (for example, 100)
  3. Set a period to take readings at (for example, 10 milliseconds)
  4. Set a minimum and maximum expected value for the ultrasonic readings
  5. Take a reading
  6. If it falls in the expected range, add it to the “total” value, and increment the number of “good” readings you’ve taken
  7. Sleep
  8. Repeat 5-7 until either you’ve taken the target number of “good” readings, or you’ve reached the maximum total number of readings
  9. Divide the “total” of the “good” readings by the number of good readings that were taken, to get the average. This is now your “filtered value” for the ultrasonic.

I hope this is a fairly straightforward explanation. By only taking “sane” values from the sensor, you can eliminate noise caused by inconveniently placed field elements and other sources in some scenarios. Further tweaks to this algorithm can be made, for example running it concurrently on two separate ultrasonic sensors that you happen to need the values of at the same time, and falling back to an alternate sensor data source if there weren’t enough “good” readings; we did both of these last year.

Best of luck and feel free to ask if anything here needs clarification.