I've been working on a slightly different way to handle the main loop() in Nybble's code. Basically the idea is to move control of the main loop off the nyboard and onto the raspberry pi, so that we're not quite so constrained by the amount of code space on the nyboard. I wanted to reduce the functionality of the arduino code to only doing I/O, and let the pi handle all the smarts.
To do that, I've been working on this fork: https://github.com/regularfry/OpenCat/tree/acy-data-loop
If you have a look, you'll notice a few things going on in OpenCat.ino. First, the guts of the loop() function are mostly gone, replaced with the excellent protothreads suggestion from @Gero . I've nicked the ultrasound implementation and moved getYPR() into a background thread. checkBodyMotion doesn't exist any more; I want anything that smart happening off-board.
There's a new file called interpreter.h, which is a different take on the serial protocol. Rather than stay with an ad-hoc syntax, I've implemented a very simple FORTH-syntax repl; I haven't got as far as allowing loops or user-defined functions yet, and I'm not sure there's enough RAM to do so, but it's on my list to try. What that all means is that if you run it, on the serial line you'll see this:
* Start *
Initialize I2C
Connect MPU6050
Test connection
MPU successful
Initialize DMP
Checking hardware revision...
Revision @ user[16][6] = A5
Resetting memory bank selection to 0...
1148 -61 -33 12
Enable DMP
Enable interrupt
DMP ready!
* Assigning 4 skill addresses...
1
2
3
3
3
3
3
3
3
Finished!
> That is, there's the usual boot and a mildly tweaked setup() routine, then it drops you straight to a prompt. From there, with the serial line set to "Newline", you can run commands like:
0 8 mThat is the equivalent of "m 8 0" in the conventional serial syntax. Or:
0 8 m 0 12 m 0 9 m 0 13 mThat zeros the front two legs in a single command. Or, shorter:
0 0 0 leg 0 0 1 legwhich does the same thing. There's also a "pose" and a "legPose" command, which set either all the motors or just the walking motors all at once:
-30 -80 -45 0 -3 -3 3 3 60 60 -60 -60 -45 -45 45 45 poseThat would be the equivalent of the existing "rest" command. After each of these, you get dropped back to a "> " prompt.
If you want to read Nybble's sensor state, you've got the "p" command:
> p
t 4710
b 273.00
g 0.09 -0.08 0.99
u 6.05
m 256 256 256 256 256 256 256 256 256 256 256 256 256 256 256 256
--
> What that's giving you is the timestamp, battery voltage, gravity vector, ultrasound range, and motor angles (with 256 being used as an "off" signal in this case). It'll also give you raw IR data if it picks a signal up.
With just the gyro in the loop, before I switched to protothreads, I could happily run a 20Hz update loop, with python on the host requesting state, doing calculations, and issuing motor commands, with about 20ms to spare per cycle. I haven't done any timing yet, but I think I'm right in saying that protothreads make this safe with ultrasound and the buzzer thrown in, and I might even be able to go a bit faster. At the moment, the motor updates are happening in the same protothread as the serial processing, and if I separate them out I can probably run the motor updates at a higher rate to smooth them out and get some interpolation back. I need to double-check that I'm not going to get into trouble with stack usage, though.
The downside of all this is that I've pretty much thrown out all the code around instincts, behaviours, and gaits. They should be easy enough to add back in on the python side - after all, it's just a matter of putting the right commands together from the data in InstinctNybble.h and feeding them over the serial line - but that's probably not next on my list; I'm more interested in getting to a walking cycle that's less pre-canned and more calculated on the fly, which is the whole reason for bothering with any of this. I'm pretty sure we can get to a dynamically stable trotting gait with a following wind (although not hopping, I don't think the motors will take it). I've got him balancing (very, very shonky feedback control works disconcertingly well), but I managed to overload an MG90D doing knee duty in the face of some rather over-enthusiastic gain factors and have been waiting for more servos in the post for the last couple of days.
Have a poke, have a play, let me know what you think. I'll commit some example python that uses this protocol in the next couple of days, and *possibly* some erlang or elixir if I get enough time over the weekend. Right now I'm developing all that on the pi over ssh, which is a lovely way of working.
Oh, one other thing - this almost certainly breaks Bittle in horrible ways. Looking at the shared code I suspect the two could be brought into line fairly easily if there was interest, but I've not got one to check against.
To answer your question about feedback control, all I've done as a sort of proof of concept to show anything was possible at all is to take the gravity vector and use it to change leg lengths, in a very approximate way - Nybble balances on a tilting book up to a good 30 degrees off-horizontal just by using the components of the gravity vector away from vertical along the x and y axes *directly* as error signals, you don't even need to do any trig. Just a gain factor of about 0.05 so the movements aren't too violent. That does limit the speed he can react at, so it's shonky as hell, but like I say - proof of concept :-)