C++ Balancing Bot
DavidZemon
Posts: 2,973
I thought I'd start a project page for this thing. I'm having a lot of fun - it's nice to finally write an application for the Propeller instead of just a library.
Architecture
Propeller running PropWare-based code does a few things: 1) reads from the gyroscope (L3GD20) and accelerometer (ADXL345), 3) computes tilt angle, 4) runs PID algorithm, 5) drives the motors, 6) logs data to SD card (might get cut due to code size restraints) 7) receive directional commands
^
|
I2C bus
|
v
ESP8266 Huzzah running Micropython relays directional commands from WiFi to Propeller. Might be capable of relaying logging information (such as current tilt angle) back to the controlling device
^
|
UDP socket over WiFi
|
v
Android phone application can set the trim (to compensate for unbalanced chassis or poorly mounted accelerometer) and send directional commands via a virtual joystick
Source Code
Propeller code is here: https://github.com/DavidZemon/MiniSegway
ESP8266 code is yet to written
Android code will be posted soon
Propeller Software Architecture
I'll describe the software architecture by cog. At the time of this writing, the following cogs are running:
Sensor Reader
Status: Complete
Loop at 250Hz, reading a single value from each of the gyroscope and accelerometer. Scale the accelerometer to floating point number with its units being g's. Scale the gyroscope to a floating point number with its value being in degrees per second. A global boolean is set high at the end of each loop.
Angle Computer
Status: Complete
Wait on the global boolean from the sensor reader to go high. Clear the boolean, then compute the robot's current tilt angle based on the previous angle and the current sensor data. Taking into account the trim, compare the current angle with the maximum allowed angle and throw an error if the angle is too great.
Message Receiver
Status: Complete
Run an I2C bus as a slave, waiting for incoming messages from the ESP8266. Save the messages to a global variable and set a global boolean high.
Message Handler
Status: Complete
For a received message, then clear the global boolean for message received. Determine if the message sets trim or requests movement. If trim, adjust the trim in memory, save the updated trim to EEPROM. If movement, adjust the ideal tilt angle and turn power.
PID Loop/Motor Controller
Status: In progress
*NOTE: This may get absorbed into the angle computer cog.
Run the PID loop against the trimmed tilt angle. Use the PID output to compute the necessary power for balance, then adjust the value for each wheel based on the turn power. Set motor direction pins for each motor. Save each motors' power to a global variable.
PWM Driver
Status: Complete
Run two PWM waves at 20 kHz. Read the motor power variables from the motor controller cog and update the PWM duty cycle.
Console Logger
Status: Complete
Print various tidbits of information which are available via global variables over a serial (UART) console for debugging.
Parent
Status: Complete
Waits patiently for the a global error flag to get set. If an error is thrown, the parent cog shuts down all other cogs and blinks a WS2812 LED with a unique color depending on the error.
Architecture
Propeller running PropWare-based code does a few things: 1) reads from the gyroscope (L3GD20) and accelerometer (ADXL345), 3) computes tilt angle, 4) runs PID algorithm, 5) drives the motors, 6) logs data to SD card (might get cut due to code size restraints) 7) receive directional commands
^
|
I2C bus
|
v
ESP8266 Huzzah running Micropython relays directional commands from WiFi to Propeller. Might be capable of relaying logging information (such as current tilt angle) back to the controlling device
^
|
UDP socket over WiFi
|
v
Android phone application can set the trim (to compensate for unbalanced chassis or poorly mounted accelerometer) and send directional commands via a virtual joystick
Source Code
Propeller code is here: https://github.com/DavidZemon/MiniSegway
ESP8266 code is yet to written
Android code will be posted soon
Propeller Software Architecture
I'll describe the software architecture by cog. At the time of this writing, the following cogs are running:
Sensor Reader
Status: Complete
Loop at 250Hz, reading a single value from each of the gyroscope and accelerometer. Scale the accelerometer to floating point number with its units being g's. Scale the gyroscope to a floating point number with its value being in degrees per second. A global boolean is set high at the end of each loop.
Angle Computer
Status: Complete
Wait on the global boolean from the sensor reader to go high. Clear the boolean, then compute the robot's current tilt angle based on the previous angle and the current sensor data. Taking into account the trim, compare the current angle with the maximum allowed angle and throw an error if the angle is too great.
Message Receiver
Status: Complete
Run an I2C bus as a slave, waiting for incoming messages from the ESP8266. Save the messages to a global variable and set a global boolean high.
Message Handler
Status: Complete
For a received message, then clear the global boolean for message received. Determine if the message sets trim or requests movement. If trim, adjust the trim in memory, save the updated trim to EEPROM. If movement, adjust the ideal tilt angle and turn power.
PID Loop/Motor Controller
Status: In progress
*NOTE: This may get absorbed into the angle computer cog.
Run the PID loop against the trimmed tilt angle. Use the PID output to compute the necessary power for balance, then adjust the value for each wheel based on the turn power. Set motor direction pins for each motor. Save each motors' power to a global variable.
PWM Driver
Status: Complete
Run two PWM waves at 20 kHz. Read the motor power variables from the motor controller cog and update the PWM duty cycle.
Console Logger
Status: Complete
Print various tidbits of information which are available via global variables over a serial (UART) console for debugging.
Parent
Status: Complete
Waits patiently for the a global error flag to get set. If an error is thrown, the parent cog shuts down all other cogs and blinks a WS2812 LED with a unique color depending on the error.
Comments
I have the gyro and accelerometer hooked up and providing reasonably decent tilt angle values @ 100 Hz. All of the parameters that go into computing the angle are being logged to an SD card. A safety has been coded in such that, if the angle exceeds 15 degrees (positive or negative) the system will halt and the SD file will be flushed, closed, and filesystem unmounted. If any errors occur (only two are coded at the moment: SD/FAT error and excessive tilt), a unique color will blink on a WS2812 LED. The overall tilt angle is also logged to the main serial console.
The Android app is coming along nicely. It is able to communicate with a local Python instance (the Python app is just dropping everything to stdout), it has a joystick for controlling movement and the trim buttons are in the works. My wonderful wife is in charge of this part of the project.
The ESP8266 code hasn't come very far. I started looking into getting the I2C communication working but was not able to get reliable comms up via PropWare's I2CSlave object.
Next up
The chassis will get built in two weeks. I'm looking at something that will be 10 inches wide and between 18-24 inches tall.
I could start coding the PID loop, but that's not very exciting without a motor controller (should be arriving in the next week or so). I'll probably get back to work on communication between the Prop and the ESP8266, seeing as how that is another extremely critical piece of this project. I'll likely need to drop SD logging while working on that... I need a smaller SD/FAT object.
I haven't used PropWare as of yet, but this might be a good reason to do so.
I'm interested in seeing how this project progresses.
For a case like this I think a home made, application specific, protocol would be better.
These graphs are from the system while sitting still on my desk.
The top graph's y-axis is computed angle and the x-axis is just point indices in the data. Each point is about 30ms apart. The high amplitude and shorter frequency wave seems to be at about 1.6 Hz. The bottom graph is the DPS from the gyroscope only (and zoomed in significantly relative to the top graph). The frequency of this wave matches quite nicely with the frequency of the top wave, at about 1.6 Hz. I worked at length tonight trying to see what could be done to alleviate that. Turning the high-pass filter up from 0.9 Hz to 3.5 Hz helped, though I may find that it does more harm than good once it's mounted on a chassis and trying to balance itself.... we shall see.
I've also done a bunch of preprocessor work to make it easier to enable/disable different logging features.
So, progress update on the code:
I2C comms on the ESP8266 aren't fun. But I think I have something that will work. Over the course of 1000 messages sent from the ESP8266 to the Propeller, the Propeller thought it successfully received 922 of them. Of the 922 decoded messages that were printed to the screen, all of them were correct. So, the good news is that I'll receive 92% of the messages that the Python board sends. And the other good news is that, if junk data makes it into the stream (bad data but valid I2C protocol), I'm extremely likely to catch and ignore it.
Bad news is... having now dropped in the code for receiving and handling messages over I2C, I'm at 23k code space. I was at 27k before I disabled the console logging that I had in there! Yikes! And that's in CMM mode!!! And I still have to program the PID loop and motor drivers. So, with 9k left I'm not worried about running out of space... but that certainly doesn't leave much room for "extras".
Oh, and the message handler code is capable of doing the following:
1) Receive message in JSON format
2) Determine if the message is attempting to set trim or move the robot
3) Handle message...
3a) If trim, trim is adjusted accordingly and saved to EEPROM
3b) if movement, new ideal lean angle and turn power are calculated based on the two vector components, magnitude and direction, in the JSON message
Anyway, Propeller code has been updated and pushed to GitHub. Time for bed.
For the Elev8-FC, I ended up pushing the drivers into the upper-32kb of the eeprom. On startup I pull them into a 2Kb temp buffer and launch the cog from the buffer. Once it's started, I reclaim the space. Serial, sensors, servos, F32, and radio code are all handled this way so it saved me a bunch of space. Looking over yours, it looks like everything is just raw C++, so that might not be an option for you.
JSON as a format is reasonably lightweight, but still probably massive overkill for what you're doing, and the code to parse it won't be small. You'd likely save a bunch of space by sending small packet-based messages with a checksum for validation. Basically just structs with an enum value at the beginning to identify them.
Are you still logging to SD? If so, the file-system overhead is going to soak up a bunch too.
For Elev8-FC, I did some tests where I compiled each of my modules out of the code to see how much they contributed, and used that as a guide for where to spend time compacting. Might be worthwhile for you too.
1) 64-bit doubles
2) Logging data to the serial console
3) JSON library
4) LMM memory model
5) All static memory allocation (meaning I have to overestimate how much space to allocate per cog, rather than using dynamic allocations and tiny stacks)
6) I2C master and slave routines (master to communicate with the EEPROM, and slave to receive messages from the ESP8266)
And Jason: SD logging was turned off a while ago due to space constraints. As much as I'd love to have it, there just isn't a chance in the world I'll be able to make it fit. Maybe if I convert fsrw someday, there's a chance... but that's going to be a future enhancement.
So, lots of things I can play with. I might be able to make this work with all the current features. All I have left is
1) Work through any bugs in the message handling (receiving is reasonably well tested, just not the handling code)
2) Add (and tune) the PID algorithm
3) Add motor controller code
The first one won't change the code size any significant amount, and the third one probably won't either thanks to the hardware counter modules. The second one though.... idk... I can probably make that fit in what's left. I have 5k right now, but I think the JSON library I'm using does some dynamic memory allocation, so who knows how much of that 5k is really and truly available for use........... hmmm.....
Anyway, I got the tracking number for the motor controller this morning, and it will be here tomorrow. I dropped one of the motors in the mail today and it will arrive at my friend's house in St. Louis on Thursday. He's printing some wheels for me on his 3D printer and is going to ensure they have a nice snug fit before I drive down Friday night. The build will happen Saturday. Hopefully I'll have motor controller software tested and working, PID algorithm ready for easy tuning, and MAYBE the full phone app will be good-to-go as well, giving me easy access to update the trim and drive the robot too. But... if I can just get the chassis built, motors mounted, and robot balancing on Saturday I'll be ecstatic.
That's a really good point. If I just scale up the angle value by maybe... 1000? .... I should be fine to do the entire PID loop with 32-bit integer math. That'll be great! That would leave me enough room to continue using 64-bit doubles with LMM, which I'd really prefer to start with (and of course I can try 32-bit doubles and CMM at a later point).
I cannot imagine why you would want to use 64 bit floats.
And heater is spot on - scale your numbers by a power of two. I typically used 4096 to 16384, depending on the precision I was going for.
Re: 32b v. 64b floats
I was afraid of the frequent rounding errors that would occur with 32-bit floats. I guess if two smart people are telling me to stop with the 64-bit, I'll stop
And it's a good thing you've convinced me now.... because I just took another look at the code and realized that I'm already using CMM, not LMM. I believe I made the switch last night after adding the messaging code. So, with all of the features listed in the above post except the memory model, I'm at 27,604 bytes. Switching to 32-bit doubles brings me down to 23,044 bytes. Turning off console logging gets me down to 19,688. Not sure about the rest of the features, as require more than a one-line change to test.
If you have a chain of a billion operations before you get to your result this can be significant. For example when weather forecasting one week in advance.
For many lesser applications, like yours, it's not worth thinking about. There are so few operations in the calculation and the errors in your measurements and actuator commands are orders of magnitude bigger.
I always like to quote my first project manager on this:
"If you think you need floating point to solve the problem, then you don't understand the problem.
If you really need floating point to solve the problem, then you have problems you don't understand"
It took me some years to realize how wise those words were.
Why did you choose PropWare C++ instead of C or Spin which seem to have a ton more libraries floating around here?
And why PropWare? Jason sure hit the nail on the head with that one! I've been developing PropWare for years now and only ever built a single project with it. I've seen people come and go with questions about how to use it, but so far no one has ever shown me a completed (or even incomplete for that matter) project using PropWare. And the one and only project I built was closed-source and for a client who ran into some personal issues at the end, meaning the project never saw the light of day. So... I have absolutely nothing that shows off PropWare in action. Until now! (or soon... not yet
And it's been quite painfully obvious that no one/not many people are using PropWare, because I've been finding a few major bugs and plenty of usability concerns with various objects as I write this code. Same thing happened with that client project I did a year back - I was able to improve PropWare significantly as I progressed through that.
Anyway, motor controller arrived today! Got the header soldered on and am getting ready to finally make things SPIN! WOOHOO!
So... I think the software is basically done. All that's left is PID parameter tuning, figuring out how to handle trim the first time the system is started after being programmed, and taking user-requested movement into account when setting the motor power and direction. BUT, the act of balancing should be complete, save for PID tuning.
No updates tomorrow evening - I'll be driving. Build happens Saturday... with any luck, I'll have a video for you all to enjoy that afternoon or evening!
I had all the code working but found that the servo was not strong enough or fast enough to keep the unit balanced.
My next attempt will be using a par of NEMA17 motors. This will require different motor code and drivers.
It's also nice to have a 3D printer to make things up as you go.
1) Make the bot as tall as possible. That slows things down. Increases the time constant as Dave says.
2) Put the heavy stuff at the top as much as possible. Well, that's why we made it tall right. Getting the center of mass off the ground increases the bot's moment of inertia thus decreasing it's angular acceleration.
3) Use lightweight wheels. This decreases their moment of inertia thus increasing their possible angular acceleration, thus increasing the effectiveness of the corrective force applied by the motors.
4) Have sticky tiers.
5) Minimize backlash in the motor drive for more rapid error correction response.
I'm pretty sure 1) and 2) are why humans are tall and thin with long legs and most of their mass higher up. Makes things easier to control. Same for four legged animals as well. Look where all the mass of a horse is, up there on top of it's skinny legs. Having a low mass at the bottom means that less power is required from the motors to get the wheels under the center of mass.
Phil, or someone, pointed out that if you want the thing to be able to propel it's self upright from horizontal then heavy wheels are required. Probably true. Or at least those wheels need weight on them to get traction. I think this is a conflicting requirement to actually balancing.
I'm praying that rubber bands wrapped around 3D-printed wheels will be sticky enough.
By the way this robot of this size does exists and does balance very nicely using NEMA17 motors. The new version uses an inverted drive belt of about 280mm and is mounted on a flat wheel with a radius of about 44mm.
Mike