You are lucky . I am the worlds expert . You need to make the mcu "kernel" have a most efficient method , so first , toss English text , and allow this kernel to work in his own way . Linking means , the kernel can most efficiently "connect" dissimilar objects and code fragments . Especially the lowest levels , will be linked 100% , so if kernel needs to improve some of his low level code , he can use these links to "figure" what went wrong . The poison of this linking is multi-tasking by forced interupts . So i never use IRQ's , i write the code to dynamically alter the perceived importance of a task . The kernel figures how much idle time the highest priority can give up and then passes this freespace to only a task that can fit in that space . This solves problems ! At idle time , kernel polls IRQ's and does so in order . But this is a small task , cause kernel knows not to poll the hardware that needs lots of space ! , kernel only polls s few that can fit in the 3 microseconds . Now you know why FIQ is NOT needed ! Cut to the chase scene , modern Forth is the only simply way to program robots .
We all want to create an algorithm to make the robot walk . Create code with a short loop . Just as your brain does , then allow a higher part of the brain develope a good habit from . More than one balance method , from stupid pendulums to rate gyros and magnetometers and VideoCams* .
and a "local" mcu to over rule the slow methods when they cant keep up .
The main mcu only needs to command and destabilize and then relax , the local mcu is accidently walking the robot , by correcting it ! Thus it is always in a balancing act . If the local mcu has good enuf "inputs" it will be able to learn how to balance better and better . It has feedback , so it knows that the fall was due to its' ignoring the faster input ! Because if it knew the "rate" better , it would have been able to correct faster ! So next time you push , it uses the rate to make a faster correction . But it wont overshoot , because its seeing the feedback of the loop . The first time , it saw the feedback did not change fast enuf , so it crashed Next time it tags that slow input as being only useable at slow rates . All of this can happen without any initial programming ! You can even add sensors , and it will characterize them and use them .
But all is lost if you bury your self in the C langauge . You cant afford such a waste of time . Especially robot s/w , must be written with a track-ball mounted on the robot , not a Host PC IDE ! No need for a big color LCD , a $10 128*64 BW from B.G. Micro .
some of my high performance ideas , use a spinning BW , low lux vid cam . It only uses alternate vertical scan line , maybe every 6th or 8th line . Each line gets a ARM9 mcu . It works like a insect eye , each line has a clock , so when that line sees a fast contrast change , its mcu can 'match" this change to its map , and send a fast signal to the main mcu , that position has changed 1 degree west . This is in the fast mode , when stuff is goin slow , the whole vid cam can be used to walk in the dark , using a few I.R. LEDS ( VidCam = .01 LUX) Trying to make ur own insect eye is actually harder than using a few lines from a $20 BW VidCam .