Mindboards

Hi everyone,

I am using Simulink, Matlab and nxtOSEK to develop an adaptive control program to guide an NXT vehicle along a predetermined path. Using modelled dynamics, the algorithm works rather flawlessly in simulation on the PC. As is often the case, reality is quite the opposite.

Keeping the starting location and vehicle orientation unchanged, there are three or four trajectories that seem to be occasionally recur while others are quite random. The vehicle generally runs at full speed in all different directions. However, one of the recurring behaviours has the vehicle move in a jerky stop-start manner. When this happens, the vehicle rotates its wheels momentarily every second or so, and may sometimes pause for a good five seconds or more. None of the trajectories is what the simulations have predicted. The jerky one is the only trajectory that seem to go in the expected direction, but only in the initial 70cm to 80cm. Even if its path-following is accurate (which it isn't), its stuttering motion means that it takes a long time to cover very little distance.

The NXT is connected to a laptop via USB so that I can monitor the positional data as perceived by the processor at every time step. So far, the data plotted out on Excel agrees rather well with the physical paths on which the NXT has travelled. That means that the sensors are working fine and the sensors' raw data is being correctly calculated to give the NXT's position. Along with the coordinate data, the clock timer reading is also transmitted to the laptop.

The program size is about 41kb, and it runs on single precision floating point with a sampling period of 0.1s. I've used a rather low sampling rate in case the processor can't handle the load. However, I've seen Segway-type and maze-solving robots that use a sampling time of 0.001s without any problem. I've read that the ARM7 processor lacks a floating point unit and thus struggles with trigonometric functions and matrix operations. There are nine trigonometric functions and 17 matrix operations in my program. I tried converting my program to a fixed-point version, but a couple of matrix operations were not compatible. Furthermore, the conversion process was a major pain, so I stopped halfway through.

If the modelled dynamics are inaccurate, the actual trajectory would be off-target. However, I would expect the results to be repeatable. The highly erratic behaviour of the NXT makes me wonder whether it's due to the processor being overloaded. It must be noted that the time step and coordinate data is transmitted to the laptop without delay. If the processor is bogged down by the program at any point, wouldn't the calculation of the sensors' data as well as the transmission of both the clock readings and processed coordinate data also be hindered?

So, is this a sign that the processor is being pushed beyond its limits? How does one tell if this is the case? If not, could someone kindly offer a reason or two for this strange phenomenon? Thanks very much.

Cheers
Hippie

I am not familiar with the nxtOSEK FW, but here is how I understand the standard FW to work (or John's EFW, which is based on the standard Lego FW). There is a scheduler that automatically switches between tasks every 1ms or 20 OP codes (or sooner if the task releases the remainder of it's time slot). These tasks consist of some FW things like LCD communication (updating the LCD), CO processor communication (talking with the AtMega48), I2C communication (talking with external sensors), USB communication (talking with a computer), BT communication (talking with other BT devices), and probably many other things. There are also user tasks, as programmed by the user, and run in the VM.

As I understand it, there is no such thing as "overloading" a processor (at least not in the sense you speak of). The processor can run OP codes as fast as the clock allows, and will not "trip up" from having "too much" to do (which I don't think could happen with the code running in a VM).

Based on the characteristics of the FW's VM, making lots of tasks will just slow down program execution speed. The FW should keep running the necessary tasks (like FW motor control).

I suggest that you need to build you program with a different structure. If you have multiple tasks, make sure everything is thread-safe (which includes using mutexes where necessary).

You can't overload a processor. A processor executes instructions and it will do that as fast as it can. (Not doing anything is actually also an instruction.) A processor is designed to always give the correct results and will not screw up unless you do something like overclocking.

So just let it sample as fast as it can run the processing, which I highly doubt takes 100 msec. Anything else is a waste.

What happens if you change the update rate in the algorithm to .2s from .1?

Is the path the robot supposed to take predictable? That is, if you have a specified starting point and orientation as well as an end point, can you predict the path?

Is there supposed to be any randomness in the path taken? If not, why did you indicate that there seem to be random paths?

Matt, I program in Simulink with additional Matlab code. The model is then compiled for nxtOSEK, so I don't have any low-level control of things such as threads. Indeed, I wouldn't know how it could be done even if it were possible. My programming knowledge is adequate for an engineering student, but nowhere near the level of those in the computer science or computer engineering field. Anyway, if the positional data is processed and transmitted at every sample time increment without delay, does it mean that there is no delay in the main loop as well? Surely, the main loop can't start a new iteration if there is a hold-up somewhere in the program, right?

Sebastian, my initial sampling period was 0.02s. Things weren't running right and sometimes the vehicle adopted a stop-start motion with several long pauses in between. I feared it was due to the processor being bogged down. That's why I slowed the sampling rate to a fifth of what it was. Alas, it didn't seem to make any difference whatsoever. The vehicle still runs as erratically as before and it's proving quite impossible to troubleshoot the problem when the results are so random.

James, a sampling time of 0.1s gives acceptable results in simulation. At 0.2s and above, the granularity becomes increasingly apparent and I have trouble getting the oscillations to settle. If the simulation results don't look good, it doesn't bode well for the real thing. In any case, I did try sampling times of 0.2s and 0.5s just to see what would happen. Was there a difference? The vehicle behaviour was still erratic and it was pointless comparing random results with previous random results. As for the trajectory that the vehicle is supposed to follow, it's just a straight line in this case, i.e. x = f(t) and y = 0. The coordinates are global. If the vehicle starts at a point (in global terms) that isn't on the assigned trajectory, it is required to head towards it as quickly as it can. The path it takes to get there is not predetermined. If the vehicle takes a far longer route than simulations predict or if it doesn't even head towards where it should go, I can tweak the algorithm accordingly - as long as the results are repeatable. The problem is that nearly every test run produces a different outcome.

Thanks for all our insights. If you have any other ideas, please share them. This problem is utterly mind-boggling!

Cheers
Hippe

hippicurean wrote:The problem is that nearly every test run produces a different outcome.

That generally means that you've got one or more variables that are not initialized properly. The simulation software might default to a specific value (like 0) while the actual software does not initialize it at all and you get random starting values.

On many systems, yes, that is often a concern. However, the Lego FW automatically initializes all variables to 0 (unless otherwise specified). Then again, I'm not familiar with nxtOSEK FW, so maybe it is a valid concern.

Matt, nxtOSEK programs runs natively on the brick, so variables are not initialized. I would assume that Simulink takes care of that, though I have nothing to base that on.

Hippie,
I didn't notice that you weren't using nxtOSEK directly. That could slow things down significantly, but I still doubt it wouldn't be able to handle it. If you want to check it, make it sample as fast as possible (sampling rate of 0 or 0.000001 or something like that) and monitor how much it actually manages to do.
I don't know which kind of debugging features are in Simulink, but a simple way would be to have a counter which you increment each time you run the processing and monitor that. If this number is significantly higher than the amount of samples you want to do (for a certain period of time) then there should not be any issues. If it is not, then you might consider optimizing. (Also keep in mind that the direct connection with the PC might slow it down too.)

The only thing I can suggest you is to closely examine what the program exactly is doing. Monitor for each sample the raw values from the sensor and the actions the NXT took. Monitor as much as possible, for example the sensor values after the NXT have processed them, the inputs before the trigonometric and matrix operations and the output afterwards. Is everything as it should be?
You could try mess around with the sensor readings. What happens if you use the sensor data you got from the NXT and use it in the simulation on your PC? Does it work somewhat correctly here, or will it do the same as the NXT? Or what if you remove the sensor and instead use the readings you got before on the NXT (so that there is no external stimulus.)? Will the NXT now behave the same way every time, or will it continue to be random?

spillerrec wrote:I would assume that Simulink takes care of that, though I have nothing to base that on.

Yes, you are quite right. Simulink and Matlab do initialise variables with values of zero.

Oh yeah, I found the problem! The raw sensor data is fed into a series of kinematic equations that are based on the physical dimensions and range of motion of the vehicle. Ideally, the vehicle cannot move sideways, so the equations do not take into account any lateral slippage of the tyres. However, the sensors will pick up movement in any direction. Hence, raw data that include any lateral slippage will not be calculated correctly using the existing equations. The errors will accumulate over time and the vehicle will soon lose its bearings. Running the vehicle on the polished floorboards in my home means that the tyres are constantly slipping longitudinal and laterally. Naturally, errors will add up quickly this way. That's why the NXT seemed to behave so randomly.

I had said before that the positional data had agreed quite well with the path taken by the vehicle even when it seems to go in random directions. In hindsight, I may be mistaken. Since the vehicle does not mark its path physically, I can only take visual note of its general movement. If a path charted in Excel resembles the path taken by the vehicle moments ago, I would say it's a good match for a preliminary test. In this case, the discrepancy between perceived and actual coordinates could differ by several centimetres without my knowledge. Since the vehicle goes out of control so quickly, I never let it run for long except when it goes into the stop-start mode. Had I let the test runs go for longer, the compounded errors would have been so great that physical paths would eventually look very different to the ones plotted in Excel.

Now that I've revised a couple of equations, the vehicle runs quite okay. The algorithm still needs some fine tuning to improve tracking accuracy, but the vehicle will follow the general trajectory set for it. So, Matt and James, you're both quite right that the processor wasn't "overloaded". I've tried various sampling times since, and the processor seem to handle it quite well. I'm really quite impressed with what this little processor can do! However, the tracking performance seemed to take a nosedive when I reduced the sampling time to 0.005s. I discovered that the positional data was way off, and then I remembered that it's most likely due to the sensor being limited by the NXT's 9600 kbps I2C bitrate.

The strange thing is that the stop-start behaviour hasn't gone away. It still happens every now and then. It's not a bother, but I'm curious why this happens. It's almost like the processor is experiencing a brain freeze.

Thanks to both of you for your valuable insights. Now I can concentrate on improving the algorithm without worrying about processor issues.

Cheers
Hippie

hippicurean wrote: Now that I've revised a couple of equations, the vehicle runs quite okay. The algorithm still needs some fine tuning to improve tracking accuracy, but the vehicle will follow the general trajectory set for it. So, Matt and James, you're both quite right that the processor wasn't "overloaded". I've tried various sampling times since, and the processor seem to handle it quite well. I'm really quite impressed with what this little processor can do! However, the tracking performance seemed to take a nosedive when I reduced the sampling time to 0.005s. I discovered that the positional data was way off, and then I remembered that it's most likely due to the sensor being limited by the NXT's 9600 kbps I2C bitrate.

Cheers
Hippie

Note that the communications between the NXT and the AVR (for reading the motor position) occurs every 2 ms or so (See the Hardware Development Kit Docs pg 18).
The NXT <-> AVR I2C link is different from the I2C communications used to interface with the other sensors via the Sensor Ports. Hence there is a lower limit to your sampling interval, beyond which any update to the motor output won't have any effect.

Nonetheless, I feel that your use of the term "overloaded" is valid, since if you can't complete your algorithm processing before the next sampling interval, effectively you'll miss the next sample (hence overloaded). This has to do with real time processing requirements of your application.