Need Help with Balancing Robot
amalfiCoast
Posts: 130
in Robotics
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?
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);
}
}
Comments
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
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?
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
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
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
"(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.
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
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?
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.
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
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
Thanks,
David
David