C++ Balancing Bot

2»

Comments

  • iseries wrote: »
    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

    Sounds cool! Do you have a picture of that one?
    David
    PropWare: C++ HAL (Hardware Abstraction Layer) for PropGCC; Robust build system using CMake; Integrated Simple Library, libpropeller, and libPropelleruino (Arduino port); Instructions for Eclipse and JetBrain's CLion; Example projects; Doxygen documentation
    Tag me with "@DavidZemon" if you have a question for me. I will be checking these forums far less for the forseeable future.
  • Your bot in the picture looks very nice. Certainly small bots can be made to balance. I'd be inclined to move the heavy stuff up to the top shelf.
  • JasonDorieJasonDorie Posts: 1,925
    edited August 11 Vote Up0Vote Down
    Making it taller reduces the rotation rate dramatically. (I think the relationship of angular velocity to length is N^4 - that's why small quad-rotors are freakishly nimble). Putting the weight higher up also makes the motors have to work less, and helps stabilize the top.

    My first iteration:


    My second:


    The first was too short, and hard to keep stable. For the second, I made it about 6 inches taller, and moved the battery up more. That was significantly easier to control.
  • Been a long day. The wheels are so fantastic though, I just have to post pictures before bed. Here's where I stand right now:

    Yb1-y6_3nWBGog6ot75Y_mdnuaBFdJhuHwZlOax7oo7rjc1FGypT-1v2ZyZjn-sVzYb-Kqa_Va06QcTxVjp16XiRx6ajE72J328fBJpycbCinfGMjxuV8kCtTVuMoJFNGOKi5bO38Icdd7XAbJ3E7m_K0pO_9PfCmyDmPxrnBnGUab0zn97xqQ5_iFveqeQi4gAXUy3cGjz5_ho1laDYgvRAgLNrOkpztzvp8uYedRfiou1xbxrFJtJQnDDCI2amrgxZD8ClaMR-VJXa2fb-OpRVHrRVeU-4j73WH11UJ6y64SAvShnu3CnX8KZXoGWyyDk9k7pLehFSyaZ6DYyPLxfJD4l1NPO46TgvBfpl-gIxzhXIOZw27kl0Wtyry2BkvJakm3nyU7GPeqRmA-2-qomgtshanCj4cO77qLXETnXYBQkR3zUIWEkOKlwISHKSoQ74FtE95uxcm6sJL9no9BgVTswtirFQWG3J0aeyji4M3ZOE4awZiiadeKEqMY0fTr2NzTiU_IhrZ0-p_waUfl7OlBZU4MBrF3NSpX6knKmqhxV-hrwIhwfy54sA2fJQxIl0BI9-Rotlx_J2zoM9pHrIHy_iV2tBA28OuASXAdyeR5ubrBV3TF2K5P-DJU0WeKVAzw1LWgPDvq6EUgsBprV7fE1GPTUluxVMwLDrx3unFg=w1283-h962-no

    YGFJiYbBbaFo5Mem3a8pYS2gsxZVEwnAilE4rTiLfGM4iUEoLT8RYTouvnJQH-ZHEJ50Ir78KLf9XqRY_ctNLXBiCSMDlJnlxUaHkdkUjZz5Cd10ABcVCilMybpcqQKZJtrue0emlGJn4aNxeeFT5uVQ5JFzILp4ShmO6qdj0ezisOQmqhe6awJvHuiLSIrKPmaxnhi2NkpUJY5KLPkZSJYxuJibuDJcf0llCJeEt_NaBS8fagask_USlsxNL6dAL4FVV4RdGAuZ5CL4MW1Zj4jVFSEZlJKEn_tLViUeFxT6HnR8VJjzZ8R2520AauB3Dnw0BFu4hVdCKodhVdHFD7PWzKPXZzTJGZhXYMAD8pEv5Ok2ZiAdqCs8vo4PczuIi_BZ6A72PyzaEerW2uYhrhWdVkFvWLmyd3jswtYOQVMtA2nNn6WuqslZhLcCwEkHz_XVQmYOq83RvT_NDFVKEWPUznePoFSXp2OAluphVB3rl1pEEMDMJB2NqUAdY0RDC1q_6Phxgt__yn1ETcNAM5igli-Anm-0d1Rm8w9dGLFsnBy_VwJO2iZ7RXeciiSfTiFQgz8BGOHHFAQcwrm_oI0n9d5aZkJtJYgX7ujv_41hKdU7UCqtKg0DhQ_krqC5bRVUMXjne-kyTxjEP1Tdw6U8_6WuRNyjgCZJyAIBFpB4Iw=w1283-h962-no

    EniDZCcUBB1_2DCUR0p9g46G_WUdfMBvjfVlW64ddLgeVnrZxBU4k0hu7tNnqSJTsJXXQfEYyWUO9v_NkM4nAao884esm8VA32XtxIGWQHLmZPKrDRwJcS5czH5f0OFbFecApDVgNNPpgv0DlxBWdEM8euFCQdt2CAjufIFvdoZYxfqGzJmMGThOPQEEJSIhpi622qxaZXYjvfbe_5BTSYctAkCsGJHc7vIO3KPlhIaiFair_IqMU-xWUgPfWbN5k2lJG0bSrmKZ12BYfpV8q-mmkxQTzq3ZzCnL6Y7z68eoIrn7KhTdF5LjLRlxzkY3ZuN3139wUo4lQhyrAvjXZ2EfbvPWn3SNGhL2yd6ouv3j3Fz0fOeT6Pgi-3ugmF8tbEOE-GZ8cDefxG13TsAMwBHl3EhrOG66SunXi0TYuhB4zORbjMxJBKz_HL7IW2p0BbKFXn-Z3xO5SVLRdwdzf-Ooza4hh8BQl39yieixMmLCyMWGRBzTzQ2wen-8OHTfY3VF9Zlz6kwOxvBGxbulxV62_IbtrQrFVX6gWdowtcoRaQmDJvz2ubUTL4h8Q-7OmhXS9ILWl3MuIu5erNCjluOB6k-uZIRi6V4sX5nGFWbwZYSvpoB_ZGNMIK67cS5RULlt941xONbjKZy1-G8lV4XnaF7ZinkLqVtMuVu09HieBQ=w1283-h962-no
    David
    PropWare: C++ HAL (Hardware Abstraction Layer) for PropGCC; Robust build system using CMake; Integrated Simple Library, libpropeller, and libPropelleruino (Arduino port); Instructions for Eclipse and JetBrain's CLion; Example projects; Doxygen documentation
    Tag me with "@DavidZemon" if you have a question for me. I will be checking these forums far less for the forseeable future.
  • Oh yea, so I mentioned before, I'll be using rubber bands for tread this weekend. But long term... I have no idea. I'd love to hear some ideas for what you might use in this scenario.
    David
    PropWare: C++ HAL (Hardware Abstraction Layer) for PropGCC; Robust build system using CMake; Integrated Simple Library, libpropeller, and libPropelleruino (Arduino port); Instructions for Eclipse and JetBrain's CLion; Example projects; Doxygen documentation
    Tag me with "@DavidZemon" if you have a question for me. I will be checking these forums far less for the forseeable future.
  • I suspect the Parallax Bands are a bit too small for these wheels. Considering that where the bands will go is smooth and has no groove for the bands to sit in, you might want to go with an over sized rubber band that is wider than the width of the wheels otherwise it might just slip off.
  • Here is the inverted belt drive option

    WheelBelt.JPG

    Mike
    2272 x 1704 - 1M
  • We use the Asparagus rubber bands for a lot of different things here.
    Founder of Kinvert
    https://www.kinvert.com/
  • We use the Asparagus rubber bands for a lot of different things here.

    Good idea. Perhaps we'll have asparagus for dinner tonight!

    So the chassis is built and electronics are mounted! Exciting!

    TjGMFV9ixDuGOWbmRdJp5f_w_dKW9E46-DFRQ5gKY5fpv6oSSHuI9emlW4RFawdQKuK8OLMpGL3_IEoRUTTXzC91LnTaUUiwTjod5-CdwDh0b3BOSOGUKZ16ld8j5lNrnKM6mTc-utAaOYDfsnmcULmRay16p8CtAqZ6nH0K4UAp0PyBi3c0wfGpw0Us4MWpLbZIrVziQ4kAbCbXyGehsZTpHwkb_paV4zW87zIW5aywAvQ-3brcf4s4bDjE4ffDuorbdFfWgz0Bl-oSGFNiJA1285pt3A8f_S-zlnirH8Gw4V-tE4R-S6zX2POA4HWa7elYMytheFYhEm0HaxfoeSlDMz_NP4jlht_xS9xhLlam06Q8kwbng6_bGM3p-8NXOS7hQuQf_slWT33XFfWZillrvuF-ncX-bXG4NfUwHTs52ob_ho1iUUTMagkcpu7GhpBATfKAt5FWK3qq-HggEhI1nCQTgbOztXyxjSP8KtiwjhGLQmSld_WOgTHc1P3YVZV_jN2JGeuAWqRAUjgVU2hAOvseKeBm9m_5qhSVhit7GEgJtoAOCLKK1ypKTFIh3Mb179dcjfc85ObsLuSzLqG_Uw41ZzookXr14P_ErW0oLnMIiiWy=w722-h962-no

    Unfortunately... one of the motors has way more resistance than the other (I can feel it when I spin the motor by hand, and the motor doesn't start spinning until after the other one). Not sure what to do about that. I can certainly try and come up with some software that compensates, but that seems like a very non-ideal solution.

    Other than that, I just have to tune the PID constants now.
    David
    PropWare: C++ HAL (Hardware Abstraction Layer) for PropGCC; Robust build system using CMake; Integrated Simple Library, libpropeller, and libPropelleruino (Arduino port); Instructions for Eclipse and JetBrain's CLion; Example projects; Doxygen documentation
    Tag me with "@DavidZemon" if you have a question for me. I will be checking these forums far less for the forseeable future.
  • DavidZemonDavidZemon Posts: 2,378
    edited August 13 Vote Up0Vote Down
    Well.... this has been a disappointing afternoon. The robot is built and the software doesn't seem to have any bugs... but it doesn't run for crap. I just tested the center of mass and it's only about 1/4 the way up the chassis... that probably isn't helping. These motors are way too big :(
    We have some lead weights and various steel beams, angle brackets, rods, etc that we might try adding tomorrow to get the center of mass higher up. The chassis that you see above is entirely aluminum, and the battery pack is just 8 AAs, so as you can imagine, there isn't much mass anywhere except in the motors.

    On top of that, I seem to have about 1 degree of slack in each of the motor shafts. I imagine that's not helping either :'(

    Tomorrow morning I'll see about making this taller and moving the sensors higher. With added weight and height, we might be adding some braces to combat twist.

    I'm really glad I drove down to my dad's place, where he has all the tools and scrap to make this happen. So far we've only made two trips to the hardware store, and that was just for bolts & nuts to mount the motors and brace.
    David
    PropWare: C++ HAL (Hardware Abstraction Layer) for PropGCC; Robust build system using CMake; Integrated Simple Library, libpropeller, and libPropelleruino (Arduino port); Instructions for Eclipse and JetBrain's CLion; Example projects; Doxygen documentation
    Tag me with "@DavidZemon" if you have a question for me. I will be checking these forums far less for the forseeable future.
  • Heater.Heater. Posts: 19,855
    edited August 13 Vote Up0Vote Down
    A rather suspected this might happen.

    How are you measuring the tilt angle from vertical?

    What actually is your control algorithm?

  • Reading from the sensors and computing the angle has been merged into a single cog now:
    this->read_accelerometer(g_accelValueAcosAxis, g_accelValueAsinAxis);
    g_gyroValue = this->read_gyro();
    
    
    const double accelerometerAngle = to_degrees(g_accelValueAsinAxis, g_accelValueAcosAxis) - ACCELEROMETER_OFFSET;
    const double leanFromGyro       = g_angle + g_gyroValue / SENSOR_UPDATE_FREQUENCY;
    
    g_accelAngle = accelerometerAngle;
    g_gyroAngle  = leanFromGyro;
    g_angle      = accelerometerAngle * ACCELEROMETER_WEIGHT + leanFromGyro * GYRO_WEIGHT;
    

    No calculations are done in the first two lines, other than to convert the accelerometer to units of gravity and gyro to degrees per second.

    The to_degrees function simply evaluates to "atan2f(par1, par2) * 180 / M_PI".

    ACCELEROMETER_WEIGHT is hardcoded and GYRO_WEIGHT = 1 - ACCELEROMETER_WEIGHT. I've 10% seems to be the most stable value for the accelerometer right now, which is quite a bit higher than the 2% Jason was recommending.

    I do have independent offsets for both the gyro and accelerometer. I'm using the gyro offset to counteract the constant motion that i'm getting when the system is stationary, and the accelerometer offset as a trim.

    I then run the PID loop in a separate cog (based on the runtime and desired frequency of 250 Hz, it could be the same cog... i just haven't merged them yet). Its code isn't too complex either:
    auto pidResult = this->pid(g_angle - g_trim);
    
                    if (pidResult > 0) {
                        leftMotor.clear();
                        rightMotor.clear();
                        pidResult += MOTOR_DEAD_ZONE;
                    } else {
                        leftMotor.set();
                        rightMotor.set();
                        pidResult -= MOTOR_DEAD_ZONE;
                        pidResult += DualPWM::MAX_DUTY;
                    }
    
                    g_leftDuty = g_rightDuty = static_cast<unsigned int>(pidResult);
    

    And the pid() method:
    int32_t pid(const double currentAngle) {
        static double previousAngle = 0;
        static double integral      = 0;
    
        // Proportional
        const auto error = currentAngle - g_idealAngle;
    
        // Integral
        integral += error;
    
        // Derivative
        const auto derivative = currentAngle - previousAngle;
    
        previousAngle = currentAngle;
    
        const int32_t output = static_cast<int32_t>(KP * error + KI * integral + KD * derivative);
    
        g_pidError      = error;
        g_pidIntegral   = integral;
        g_pidDerivative = derivative;
        g_pidOutput     = output;
    
        return output;
    }
    

    If you need more context for any of that, see SensorReader.h, PIDController.h, and PWMDriver.h for the core functionality: https://github.com/DavidZemon/MiniSegway
    David
    PropWare: C++ HAL (Hardware Abstraction Layer) for PropGCC; Robust build system using CMake; Integrated Simple Library, libpropeller, and libPropelleruino (Arduino port); Instructions for Eclipse and JetBrain's CLion; Example projects; Doxygen documentation
    Tag me with "@DavidZemon" if you have a question for me. I will be checking these forums far less for the forseeable future.
  • Oh, and I brought special attention to the 10% accelerometer weight being different than Jason's recommendation not because I think Jason was wrong, but because I'm hoping that helps anyone reading this have some idea of what might be going wrong.
    David
    PropWare: C++ HAL (Hardware Abstraction Layer) for PropGCC; Robust build system using CMake; Integrated Simple Library, libpropeller, and libPropelleruino (Arduino port); Instructions for Eclipse and JetBrain's CLion; Example projects; Doxygen documentation
    Tag me with "@DavidZemon" if you have a question for me. I will be checking these forums far less for the forseeable future.
  • First thing I'd want to check is what is the current_angle being fed to the PID? Is it correct?

    Does that current_angle read correctly as the stationary bot is tilted forwards and back?

    Does the current_angle read correctly when the bot is tilted at some constant angle and being accelerated horizontally forward and back?

    I would suspect the latter goes a bit screwy under horizontal acceleration.

  • Heater. wrote: »
    First thing I'd want to check is what is the current_angle being fed to the PID? Is it correct?

    Does that current_angle read correctly as the stationary bot is tilted forwards and back?

    Does the current_angle read correctly when the bot is tilted at some constant angle and being accelerated horizontally forward and back?

    I would suspect the latter goes a bit screwy under horizontal acceleration.

    I have a UART logger that prints a line every 30ms. At slow tilt speeds (moving by hand), it is pretty accurate. I have definitely noticed that angle is drastically affected by lateral acceleration.

    I just found out that we have a Microcenter here in St. Louis, so I'm off to go buy a BNO055 and use its algorithm for angle. If I can get that working I can tune the PID algorithm. Then I might go back and try to figure out my own angle algorithm for the fun of it... but the BNO055 should get me started hopefully.
    David
    PropWare: C++ HAL (Hardware Abstraction Layer) for PropGCC; Robust build system using CMake; Integrated Simple Library, libpropeller, and libPropelleruino (Arduino port); Instructions for Eclipse and JetBrain's CLion; Example projects; Doxygen documentation
    Tag me with "@DavidZemon" if you have a question for me. I will be checking these forums far less for the forseeable future.
  • I was just going to suggest a BNO055 lol.

    Not sure if anyone has drivers for it yet that will work with the Prop. But we bought a couple anyway about a year ago in anticipation of our balancing bot.
    Founder of Kinvert
    https://www.kinvert.com/
  • There is the fundamental problem.

    The accelerometer gives a great indication of orientation when stationary, thanks to gravity, but is messed up when things are accelerating.

    The gyro gives a great indication of rate if change of orientation no matter what. But drifts.

    Somehow these two sensor inputs can be fused to arrive at a more stable orientation. I have never really got my head around this but I did get this sensor fusion code working very well as while back:
    http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/

    There is a C version of the imu algorithm. Not sure if it will fit in a Prop though.

    I'd be very interested in how you get on with the BN055. I have been meaning to get hold of one to play with for ages. Basically it does all that IMU fusion for you. Sounds great.

    My crazy Javascript version of Sebastion Magwick's IMU sensor fusion is here (Including original C source):
    https://github.com/ZiCog/madgwick.js






  • Thanks for the links Heater..

    Got the BNO055 in front of me now. Time to get to work creating a driver in PropWare.
    David
    PropWare: C++ HAL (Hardware Abstraction Layer) for PropGCC; Robust build system using CMake; Integrated Simple Library, libpropeller, and libPropelleruino (Arduino port); Instructions for Eclipse and JetBrain's CLion; Example projects; Doxygen documentation
    Tag me with "@DavidZemon" if you have a question for me. I will be checking these forums far less for the forseeable future.
  • Sensor fusion in 1D is pretty trivial, which is all you need for a balancing bot. I gave pseudo code for it in another recent thread, here:

    http://forums.parallax.com/discussion/comment/1417473/#Comment_1417473

    The one I showed above was using the full IMU I developed for my quads (which is what the Elev8-FC's IMU is derived from), but that's complete overkill for something like this. A simple complimentary filter should be plenty.
  • Thanks Jason.

    Your complimentary filter pseudo code is simple enough that even I can understand it.

    It looks like some parts of a 1D Kalman filter, where inputs from different sensors are mixed in various proportions to get a more accurate estimate. Except in the Kalman the proportions are changed dynamically depending on how much you trust them at the time.

    Not that I have looked into or understood Kalman very much.

    It's all moot now. All of this is built into the BNO055.


  • Keith, David, Heater
    The BNO055 module is great. I can recommend it for experimentation and development.
    I was guided by rjo's excellent work - see this thread;
    http://forums.parallax.com/discussion/163121/adafruit-9-dof-breakout-for-quads.



  • Jason, the algorithm you posted is exactly what I'm already doing. Seems like it should work, which is part of why I got pretty disappointed yesterday when it refused to balance. Putting a bit of lead at the top seemed to help a little, but I still couldn't get it right. Anyway, I'm just dead tonight. I was only able to give the robot a couple hours of attention all day. Tomorrow I drive back to Omaha first thing in the morning, so I sure hope my chassis doesn't need any more significant work.... It will be a lot harder from here on out. When I get back, I'll replace my sensors with the bno and try tuning the PID constants again. Only once I get *that* working will i go back to the original sensors to see if I can make it work. I still fear the slack in the motors is hurting me, and I don't know if the center of mass is high enough yet (though I think it is)
    David
    PropWare: C++ HAL (Hardware Abstraction Layer) for PropGCC; Robust build system using CMake; Integrated Simple Library, libpropeller, and libPropelleruino (Arduino port); Instructions for Eclipse and JetBrain's CLion; Example projects; Doxygen documentation
    Tag me with "@DavidZemon" if you have a question for me. I will be checking these forums far less for the forseeable future.
Sign In or Register to comment.