Need Help with Balancing Robot

amalfiCoast

Hello,

I've built a balancing robot that I cannot get to balance. My code is posted below. The motor control section (which I suspect is the issue) is towards the end. The robot oscillates but does not stay upright. If I turn down the kp gain, the motors do not start responding until the robot has tipped over. I have tried using the kd gain in addition to kp, but no luck yet. Also, the motors seem very fast, almost like it is all or nothing - not quite, but close. I'm using a Propeller microcontroller and Parallax gyroscope and accelerometer modules. I'm using Pololu micro metal gear motors and a Pololu motor driver. What am I missing here?

    phi=xhat[0];   //Pitch
    theta=xhat[1]; //Roll
    
    //PI w/ rate feedback
    
    float Error=theta*180/PI;
    float Error_int=Error_int+Ts*Error;
    float servo=kp*Error+ki*Error_int-kd*gx*180/PI;
    
    // Add PWM control for motors
    pwm_start(100);
    
    // Motor
    if(servo < 0){
      pwm_set(3,0,-servo);
      pwm_set(1,1,-servo);
    }
    else if(servo > 0){
      pwm_set(4,0,servo);
      pwm_set(2,1,servo);
    }            

    print("servo=%f\n",-servo);
    
  }  
}

amalfiCoast

An image of my robot is below.

iseries

I have tried to build my own balancing robot but have same issue.

The accelerometer and gyro are too sensitive to movement and without using some complex math are unreliable when the robot moves. At steady state the accelerometer is accurate but as soon as the motors move the acceleration causes large change in the accelerometer in the negative direction. The gyro is good while it is moving but can not compensate for the motors.

Mike

Hal Albach

Never worked on balancing bots, but just curious about the four pwm_set statements addressing 4 pins with two motors. Are you applying PWM to both Direction and Magnitude inputs of the controller?
Shouldn't the Direction input be just a high or low level? Also, is the optimum location for the sensors on the base or should it be somewhat higher up? Can you provide a link to the motor controller?

kwinn

IIRC Hanno Sander built a very nice balancing bot (Dancebot)using the propeller, but I cannot find much info on the project. Perhaps someone on the forum or at Parallax has some information or links they can post.

amalfiCoast

Mike,

You could try using an extended Kalman filter to fuse the gyroscope and accelerometer measurements for estimating angle. I have code I've written to implement the EKF and I plan to post everything once I get my robot working.

David

amalfiCoast

Hal,

Yes, the code sends direction and magnitude to the motor controller. I found code in one of the Learn modules that was similar but I'm not sure if I'm using it correctly. The motor controller I'm using can be found here: https://www.pololu.com/product/2135

I don't think sensor location matters because I think the angles and rotation rates are the same no matter the location. In v=omega*r, omega is constant but v increases as the distance, r, from the rotation point increases.

Thanks,
David

amalfiCoast

kwinn,

I have heard of the Dance Bot but I haven't seen much on it. Hopefully someone can chime in with more information.

Thanks,
David

Hal Albach

According to the link you provided, it appears that you are not using the controller correctly. Pololu suggests running the controller in the PHASE/ENABLE mode by tying the MODE pin to VCC through an appropriate pull up resistor (something considerably less than 100K). From Pololu;
"(Drive/brake operation usually provides a more linear relationship between PWM duty cycle and motor speed than drive/coast operation, and we generally recommend using drive/brake operation when possible.)"
They provide tables on how the output relates to the inputs, and in both tables PWM is applied to only ONE input per motor channel, not both. The other input is for motor direction and is a logic level input. If you look at the first table under "Using the Motor Driver" you will see that xPHASE is connected to a GPIO pin, meaning NOT PWM, just a high or low for direction. PWM is applied to the xENABLE for speed control.

Publison

amalfiCoast wrote: »

kwinn,

I have heard of the Dance Bot but I haven't seen much on it. Hopefully someone can chime in with more information.

Thanks,
David

Here is a guy that tried to replicate it:
http://www.smbaker.com/project-dancebot

It is based on the chapter in this book:

https://www.parallax.com/product/32316

amalfiCoast

Thanks Hal. Does this code for the motor driver seem to be correct? It seems to ramp a bit better than it did before.

if(servo<0){
      low(1);
      low(3);
      pwm_set(2,0,-servo);
      pwm_set(4,1,-servo);
    }
    else if(servo>0){
      high(1);
      high(3);
      pwm_set(2,0,servo);
      pwm_set(4,1,servo);
    }

I'm wondering if the micro metal gear motors I purchased don't have enough torque at low speeds to keep the robot upright. If I hold the top platform and tilt the robot slightly, it is not able to right itself until I tilt it further and further. What do you think?

Hal Albach

Yes, it appears to be correct if Prop pins 1 & 3 are connected to the APHASE & BPHASE. Its hard to tell from your picture - too dark.
Did you connect MODE input to VCC to enable the Drive/Brake mode for improved linear response?

Your torque concerns may be too small of a motor - their micrometal gear motors spec a stall current at 750mA with 12 Volts. With 6 D cell batteries you are probably running close to 9 Volts. The controller can only supply 1.5 Amps at 11 Volts per channel. An additional factor is the gear ratio of the gearmotor and the size of the wheels.
Another factor may be that you have the motor supply coming through the Activity Board, not sure if that may be choking the current.

amalfiCoast

I think I've solved the torque issue by removing the shelf and mounting the batteries on the bottom. It is much lighter now. It still oscillates and I'm trying to tune the controller. Still no luck yet. I set a motor minimum value so the motors spin immediately when the robot starts tilting. Do you have any other recommendations for tuning?

amalfiCoast

Hal,

I have connected the MODE pin to VCC (you might be able to see the green jumper in the above photo) and it seems to provide a better response. I purchased two of these motors: https://www.pololu.com/product/3073. Note these are the 6 V motors.

According to the datasheet the stall current is 1.5 A and from what I can tell from the Activity Board datasheet, it can supply 1.8 A, so I think I'm good there. I'm using 5 AA batteries so my voltage should be more than enough. I think my two issues are still not enough torque coming from the motors and that my controller tuning is not there yet. Any other thoughts?

Thanks,
David

iseries

I have tried fusing the accelerometer and gyro with a complimentary filter and EKF. No luck. There is just too much change when the motors kick in then can be compensated for.

You need to take into account all three axis of these units and fuse them with Quaternion's. Then use Euler angles to drive the motors.

Mike

Hal Albach

Glad to hear that your motor controller is working better, but I'm afraid I cannot help you further, never dabbled in inverted pendulum control or PID.

amalfiCoast

Is there a way to determine how fast a while loop is running? Maybe my loops are not fast enough?

Thanks,
David

amalfiCoast

Well I finally got my robot to balance but I had to use a complementary filter for its speed and low computation time. My Kalman filter is a bit too slow as it stands right now. I will continue working to optimize the Kalman filter to make it faster and suitable for my robot. Thanks for everyone's help.

David