Shop OBEX P1 Docs P2 Docs Learn Events
C++ Balancing Bot — Parallax Forums

C++ Balancing Bot

DavidZemonDavidZemon Posts: 2,973
edited 2017-08-10 04:08 in Propeller 1
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.
«1

Comments

  • Progress update

    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.
  • If you haven't already established a messaging format (or if you are just running with something custom) I'd recommend looking into Google Protocol Buffers. Google Protocol Buffers have an officially supported C++ library, but it requires the C++ standard library which from my (admittedly cursory) understanding of PropWare you don't use. As an alternative, I recommend using nanopb which is a C implementation specifically targeted towards memory constrained environments such as microcontrollers. Google Protocol Buffers also officially support Python though I am not familiar enough with that implementation or Micropython to know if it works with Micropython. A quick Google search did not reveal anything promising as far as Micropython is concerned.
  • Very nice.

    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.
  • Heater.Heater. Posts: 21,230
    Google protocol buffers are great if you have heaps of memory to play in.

    For a case like this I think a home made, application specific, protocol would be better.
  • I found a json library that "only" needs 2.5k of memory and I've already included that in PropWare with example code. I'll either go full custom or use that between prop and esp8266, depending on overall size constraints. Phone will send json packets.
  • Well tonight has been quite fun. With Jason Dorie's help I've learned a lot about how to compute a reasonable angle value. Still more to go.

    wiggle.png

    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.
    824 x 586 - 97K
  • I should really find more projects like this to work on, using PropWare. I'm finding issues left and right with PropWare's implementation - some usability, and some are outright obvious bugs. For instance, I've found out that the I2CSlave object was almost completely useless until just now. It still doesn't handle the on_request callback correctly, so it can't respond to the master... but at least it can receive, which is the more important for this project.

    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.
  • Are you optimizing for size, or speed? (Optimize for size will likely help a little bit)
    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.
  • Heater.Heater. Posts: 21,230
    I was surprised to see JSON mentioned as well. I love it for it's ease of use in the language for which it was designed, Javascript, but I think all that parsing is a bit much on a machine with so little memory. Little packets of binary encoded data will do fine here I think.
  • It was so late last night that I didn't feel like expanding on my post anymore than I already did... sorry about that. Yes, I'm keenly aware of lots of things that are massive memory hogs. As I was falling asleep I was putting a list together in my head:

    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.
  • PIDs don't have to be very big. In CMM a decent PID routine is probably only a few hundred bytes, if you're working in integers. The Elev8-FC does all the calcs in float, but then converts to scaled integers to feed the PIDs - works quite well.
  • JasonDorie wrote: »
    PIDs don't have to be very big. In CMM a decent PID routine is probably only a few hundred bytes, if you're working in integers. The Elev8-FC does all the calcs in float, but then converts to scaled integers to feed the PIDs - works quite well.

    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).
  • Heater.Heater. Posts: 21,230
    Yes but scale up your numbers by 1024 or some nice power of 2. Then your fixed point math will use shift operations instead of multiply to do the scaling. Much more efficient in both space and time.

    I cannot imagine why you would want to use 64 bit floats.
  • 64 bit floats are *really* unnecessary for what you're doing. Even 32 bit floats have 23 bits of float precision (roughly 8 million units). That's a resolution of roughly 0.00005 degrees, if you're measuring angles. That's *way* above what your sensors are going to be able to compute.

    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.
  • JasonDorie wrote: »
    64 bit floats are *really* unnecessary for what you're doing. Even 32 bit floats have 23 bits of float precision (roughly 8 million units). That's a resolution of roughly 0.00005 degrees, if you're measuring angles. That's *way* above what your sensors are going to be able to compute.

    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.
  • DavidZemonDavidZemon Posts: 2,973
    edited 2017-08-09 03:55
    Yikes. It always takes me forever to remember how hardware PWM works on the Propeller. It took me all night to put together a new object in PropWare called DualPWM, which will be in charge of driving the motor controller. And thank you both for the tip about remembering to use powers of 2 - remembering to do that allowed me to bump the max frequency from ~10 kHz to ~80 kHz. The motor controller I'm using supports 20 kHz, so reaching that frequency and avoiding any audible whine will be really nice.
  • Heater.Heater. Posts: 21,230
    edited 2017-08-09 05:25
    No matter how you do your calculations every operation will introduce rounding errors.

    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.

  • You're doing a project I've been planning on doing for some time.

    Why did you choose PropWare C++ instead of C or Spin which seem to have a ton more libraries floating around here?
  • Heater.Heater. Posts: 21,230
    No reason you can't use any C library from C++ if you want to.
  • He's the author of PropWare - it's called "eating your own dog food", which means using a real project to drive the development of your code to make sure it's complete and usable.
  • I chose C++ as the language because I don't like Spin very much. I don't like the restriction that a cog runs either Spin or PASM, never both. I also don't like the extreme language limitations. Not that I necessarily like C++, being on the other extreme end of language limitations (having almost none), but I prefer it over Spin. Also, my editing environment in C++ is somewhere between 100 and 10,000 times better than the Spin editing environment. That being said.... my editor also cost infinitely more than Propeller Tool or bst :P. And finally, I chose C++ because that is what I was trained in. It is what is familiar to me, and Spin is extremely foreign (almost as much as Forth).

    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 :D )

    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!
  • Excited to see progress in this project.
  • I've updated the first post with the software architecture for the Propeller.
  • PWM driver is complete and PID algorithm seems to be going well. I have all the electronics "ready" but I don't want to start messing with the higher voltage stuff while I'm this sleepy.

    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.
  • Short update tonight. I updated the code and wiring to work correctly with the motor driver. I'm glad I didn't attempt that last night... I would have gone to bed very disappointed without ever getting it to work, I'm sure. I wouldn't have fried anything at least, but there's no way I would have figured out the motor driver's inputs last night. Anyway, life is good now, software is ready to go, I've tested that the one motor I have in my possession turns at a speed relative to the tilt of the board and is capable of reversing direction. There's a dead-zone for low duty cycles and I'll need to fix that in software, but the general principle is working at least.

    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!
  • My attempt at building a self balancing robot failed. I tried to use common parallax parts such as Activity Board, Xbee WiFi, Parallax continuous Rotating Servos, and MPU9250 IMU.

    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.


    SelfBalance.JPG
    2272 x 1704 - 1M
  • I've never done a balancing bot, but it might help if you made the robot longer. The time constant for a short robot is very small. The time constraints get more relaxed the longer the robot is. Most people can balance a broom in their hand, but try balancing a pencil or a ruler. It's difficult to keep it balanced for very long.
  • Heater.Heater. Posts: 21,230
    edited 2017-08-11 14:01
    That's just what I was going to say. I have never built a "inverted pendulum" bot but it's been on my TODO list for years. My thinking is:

    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.




  • Heater. wrote: »
    4) Have sticky tiers.

    I'm praying that rubber bands wrapped around 3D-printed wheels will be sticky enough. :) that was the best idea I could come up with on short notice and bizarre motor shafts.
  • Correct, This one uses O rings. The wheel is printed with a cupped center so that the O ring fits around it and stays on.

    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
Sign In or Register to comment.