Balancing Bot using a Comp Filter

ShawnaShawna Posts: 481
edited 2020-03-10 - 21:06:41 in Robotics
I have been wanting to build a balancing bot for awhile now. Actually been working on the code for a long time. I finally have the bot built and balancing, so I thought I would share a short video.
All the bot can currently do is balance, but I was pretty excited last night when I fired it up for the first time and it kind of stayed in one place! The video is after about 20 minutes of tuning the PID loop.

The video is pretty boring, hopefully as I progress I will be able to post more interesting ones.

Shawn A.


  • ercoerco Posts: 19,619
    FANTASTIC! Very stable, congrats and nicely done! Not an easy task. Are those stepper motors or dc motors (with encoders, I presume)?
  • erco wrote: »
    Are those stepper motors or dc motors (with encoders, I presume)?

    I'm betting steppers.
    erco wrote: »

    I absolutely agree. Fantastic robot Shawna!

    Here's hoping the figure 8 thread gets a new entry.

    Thanks for sharing this with us.

  • Yes, very nice. I have been trying to do that for years now. Can't get it to work. I use 3D printed parts so my robot looks a little nicer.

    I have been trying to refine my stepper motor code so that it starts and stop more smoothly and for fast accelerations does not stall. I'm using 16 step mode.

  • Wow, so stable! Nice work... I've made several attempts at building one, but I just can't seem to wrap my brain around writing a decent PID loop. Any code sample, would be welcome.

  • ShawnaShawna Posts: 481
    edited 2020-03-11 - 15:40:34
    erco wrote: »
    Are those stepper motors or dc motors (with encoders, I presume)?
    They are stepper motors. I am more comfortable with stepper motors, but dc motors with encoders may have been a better route to take. I am having problems working out how to keep track of the forwards and backwards movement or travel. I know the theory of how to do keep track with steppers, but I am not sure how to code it.
    Duane Degn wrote: »
    Here's hoping the figure 8 thread gets a new entry.
    Ercos-Figure-Eight-Challenge is one of my goals.
    iseries wrote: »
    I have been trying to refine my stepper motor code so that it starts and stop more smoothly and for fast accelerations does not stall. I'm using 16 step mode.

    I was worried about the stepper motors stalling when they changed direction, but they don't. The only time the motors stall, is when the integral winds up too fast. This can be stopped by clamping the integral term so it doesn't run away. I haven't done this yet, but need too!

    I am using most of the same parts as your Roadster.

    2 - A4988 stepper motor drivers
    2 - 80x10mm wheels
    2 - Nema 17 stepper motors 1.7A. 1.7A is probably over kill and would be hard on the drivers at their rated current, I think.
    1 - 4S Lipo 2200mah battery.
    1 - Parallax Activity Board AX
    1 - Parallax WX ESP8266 WiFi Module Dip

    The code is written using SimpleIDE.
    Here is my stepper motor code which runs in its own processor(Cog). Pretty basic right now, it can only balance. Need to add turning and movement tracking to it. The PidOutput variable comes from another processor dedicated to the PID code. It may be possible down the road to combine the PID code and the Stepper code into a single processor. The PID code is basically copied from the learn site example here.
    The setpoint for the PID is an angle and sensor input is the angle estimated from the Comp Filter. The output of the Comp Filter is an Integer and the PID math is all done in Integers.
    void StepperDriver(void *par)
      int left_motor_step_pin  = 17;
      int right_motor_step_pin = 15;
      int left_motor_dir_pin   = 16;
      int right_motor_dir_pin  = 14;
      int p_PidOutput = 0;
      set_directions(17, 14, 0b1111);
      //set_directions(15, 17, 0b101);
      //CTRA = (0b00100 << 26) + 17;
      //CTRB = (0b00100 << 26) + 15;
      int loop_sync = CNT;
        // Used for Timing.
        start = CNT;
        // Transfer global varibles to local.
        p_PidOutput = PidOut ;
        // If Else statement added too shut down motors during start up or if the Bot tips over.
        if(startPid == 0)
          CTRA = 0;
          CTRB = 0;  
          CTRA = (0b00100 << 26) + 17;
          CTRB = (0b00100 << 26) + 15;    
        // Forwards and Backwards Code.
        if (p_PidOutput < 0)
          p_PidOutput = p_PidOutput * -1;
          FRQA = p_PidOutput;
          FRQB = p_PidOutput;
        else if(p_PidOutput > 0) 
          FRQA = p_PidOutput;
          FRQB = p_PidOutput;  
        // Used for Timing.
        stop = CNT; 
        timeUs = stop - start;
        waitcnt(loop_sync + (CLKFREQ / 100));       // 100Hz loop, 10mS.      
        loop_sync = CNT;

    Edit: The code above has a few things in it that need too be cleaned up to make it more efficient, but it works fine.

  • ShawnaShawna Posts: 481
    edited 2020-03-11 - 15:39:26
    Here is the PID code, which runs in its own processor. The Angle being inputted into the PID has two decimal places of resolution. If the Sensor_Input was equal to 1500, that would be 15.00 degrees.
    void PID_Control(void *par)
      int p = 0;
      int i = 0;
      int d = 0;
      int Pitch_Kp = 500; //300, 350, 300, 300, 350
      int Pitch_Ki = 50;  //100, 100,  20,  50,  50
      int Pitch_Kd = 75;  //20,   40,  50,  50, 100
      int Pitch_Current_Error = 0;
      int Pitch_Accumulated_Error = 0;
      int Pitch_Previous_Error = 0;
      int Pitch_Error_Delta = 0;
      int Pitch_error = 0;
      int Pitch_PID_Sensor_Input = 0;
      int Pitch_PID_Setpoint     = -50;
      int Pitch_PID_Output       = 0;
      cog_run(&StepperDriver, 128); // Start stepper motor driver.
      int loop_sync = CNT;
        //start = CNT;
        // Transfer global varibles to local.
        Pitch_PID_Sensor_Input = Pitch / 10;                  //Testing to try and dampen noise in the PID
        Pitch_PID_Sensor_Input = Pitch_PID_Sensor_Input * 10; //Testing to try and dampen noise in the PID    
        // Waits to start Pid until bot has been stood up.
        while(startPid == 0)
          if((Pitch < 100) && (Pitch > -100))startPid = 1;  //Start Pid once angle is between 1 and -1 degrees.      
          loop_sync = CNT;   
        // Calculate Error.    
        Pitch_Current_Error = Pitch_PID_Setpoint - Pitch_PID_Sensor_Input;
        // Calculate proportional term.
        p = Pitch_Kp * Pitch_Current_Error;  
        // Calculate integral term.
        Pitch_Accumulated_Error = Pitch_Accumulated_Error + Pitch_Current_Error;
        i = Pitch_Ki * Pitch_Accumulated_Error;
        // Calculate derivative term.
        Pitch_Error_Delta = Pitch_Current_Error - Pitch_Previous_Error;
        d = Pitch_Kd * Pitch_Error_Delta;
        // Calculate PID Output.
        Pitch_PID_Output = p + i + d; 
        // Save current error as Previous.
        Pitch_Previous_Error = Pitch_Current_Error; 
        if((Pitch_PID_Sensor_Input > 2000) || (Pitch_PID_Sensor_Input < -2000)) // Turn motors off if Bot has tipped more than 20degrees.
          Pitch_PID_Output = 0;
          startPid = 0;
          Pitch_Accumulated_Error = 0;
        // Transfer local variables to global variables.      
        PidOut = Pitch_PID_Output;
        //stop = CNT;
        //timeUs = stop - start; 
        waitcnt(loop_sync + (CLKFREQ / 100));       // 100Hz loop, 10mS.      
        loop_sync = CNT; 
  • Ironically it was a year ago that I wrote my stepper library and maybe you could use it.

    It's based on 10 hz per unit so 1 = 10 pulses per second up to about 256.

    The balancing robot that I say used two pids. One for balance and another for movement where it was only a PI. It would input the speed forward or backwards with left and right speeds.

    Anyway here is my stepper library with documentation.

  • ShawnaShawna Posts: 481
    edited 2020-03-11 - 18:07:41
    Mike thanks for the code. I am looking at your code and I like what you've done, and I have a few comments.

    I like what you have done with basing each increment to a know hz value. If I am not mistaken that makes the stepper motor response linear, which I think is desirable.
    I am still trying to figure out how you came up the multiplier. Currently digging through the AN001 Counter data sheet.
    The multiplier in your library is 5369. I think that comes up with a minimum frequency of 100Hz not 10Hz. If my math is right, that maybe why you where having problems with the bot balancing and the motors stalling.

    (5369 / 2^32) * 80,000,000 = 100.00541

    I maybe missing something here.

    If your PID ouput is 0 the frequency would be 0hz. Then if the PID output changed to 1 the frequency would be 100hz. That's a pretty drastic change I think!

    My stepper motor is 1.8 degrees/step. At 16x steps that's 3200 steps per revolution. 360 / 3200 = .1125
    .1125 would be degrees rotate per step at 16x step.
    So at 100hz scale an output of 1 would rotate the motor 11.25 degrees in 1 second. If the scale was 10hz it would be 1.125 degrees of rotation in 1 second.

    I maybe completely off point here!

    I'm thinking about trying something like 1hz as a minimum.


  • A scale of 537 would be 10hz and a scale of 54 would be 1hz.

    Here is the section of code from Mikes stepper driver.
    void Stepper_speed(short speedA, short speedB)
      int A, B;
      A = speedA * 5369; // 80Mhz to even 10Hz
      B = speedB * 5369;
      FRQA = A;
      FRQB = B;
      // printi("Frequency: %d\n", S);
  • ercoerco Posts: 19,619
    Thanks for all the deets, Shawna.

    @Duane Degn : Very nice to hear you weighing in, also plugging the figure 8 challenge. That reminds me that I never tried a fig 8 with my ultra-simple BS-2 balancer... have to dig that out and give it a shot sometime soon.

Sign In or Register to comment.