# Equalizing motors speed for balancing robot

I am currently working on a balancing robot project using Arduino Uno. I am able to get the robot to balance but one of its motor have the tendency to rotate more than the other.

I am using the L298N motor controller with 2 x 30:1 Metal Gearmotor 37Dx68L mm with 64 CPR Encoder from polulu.com. I have also tried to compensate for the difference in speed by monitoring the encoder value on each motor. Unfortunately, the compensation logic seems to contradict the PWM control needed to balance the robot and causes it to oscillate quite a lot.

I am currently using the following code to balance the motor speed:

dPos = leftCounterCurr - rightCounterCurr;
if(dPos > 10) {
leftPWM = u + k*abs(dPos);
rightPWM = u - k*abs(dPos);
}
else if(dPos < -10) {
leftPWM = u - k*abs(dPos);
rightPWM = u + k*abs(dPos);
}


dPos refers to the difference in the two encoders values. leftPWM and rightPWM are the PWM values to control the left and right motor respectively. u is the output of the PID control and it is adjusted by certain gain value to compensate for the difference in motor speed.

I hope someone with similar experiences can help me out. Thanks in advance.

Edit 1: @Marko I did some characterization of my motors today. Thought I would post it here in case anyone might find it useful. The results are shown below. It seems that with proper capacitance tuning, I am able to get the motors to give almost similar responses. However, I believe there is still a need to use a control loop for better performance.

For example,if the left motor has a 0.3uF capacitor across its terminals while the right motor has a 0.2uF capacitor across its terminal, we can observed that the two response curve are very close together.

Another thing that I notice was that the motor would not move for duty cycle below 20%. And it seems like a universal problem with PWM control of DC brush motors... or at least I have not find any methods to overcome this behavior yet.

• Sounds like you may want to try a velocity controller - right now, if you're error is zero, you cut your control signal to zero, and DC motors don't have holding torque when they're not being powered. – RYS Jun 13 '16 at 15:51
• @RYS I think I read a similar concept before at this link: jjrobots.com/projects-2/b-robot Unfortunately, I could not fully grasped the concept presented. I understand that the designer provides a velocity setpoint to a PI controller and the output from the controller is a desired tilt angle for the robot to maintain its balance at the given velocity. What I am unsure of is how the desired tilt angle can be computed given the velocity setpoint and the measured velocity as inputs. I hope you can share more on your idea. Thank you. – ckong80 Jun 14 '16 at 6:06

Usually a simple P,PI loop is better than bad PID loop. You can put 2 PI controlers for speed, for each wheel it own PI controller. Then you just control the speed of each wheel separately. Here is a small pseudo algorithm of PI controller with trap integration method: $u(k) = K_p \bigg[ e(k)-e(k-1)+\dfrac{T_s}{2T_i}\big[ e(k)+e(k-1)\big] +\\ +\dfrac{T_d}{T_s}\big[ e(k)-2e(k-1)+e(k-2)\big] \bigg] + u(k-1)$

EDIT 1: This is a block diagram of PMDC, notice that at the output there is an integrator $\dfrac{1}{s}$ that integrates the speed $\Omega$ into position $\Theta$. It matters if your controller uses speed or velocity feedback (or both). Since the veloctity feedback is obtained derivating the position, that's how is usually done, you can use PI, else a P controller is enough good, but it will give you I like response of the entire loop P->integrating->I. Using PD will give you PI like response PD->interating->PI(IP). You should decide wheather you will use velocity or position as setpoint. Professional equipment uses combined period and frequency measuring for estimating the velocity feedback information from position feedback, so simply derivating the position will give you bad result at low speed.

IMO you should start with making a setpoint velocity generator with ramp, then integrate into position setpoint and use position as setpoint.

$v_{set}(k)= v_{set}(k) + \Delta v_{max}$
$p_{set}(k)= p_{set}(k) + v_{set}(k)\cdot T_{sample}$ integrating position
This integrating position should be exactly the same size as your counter, for example 32-bit integer, so you have to parse it in integer form. When it will roll over, this won't affect the calculation of the error: $\varepsilon = p_{measured} - p_{set}$

EDIT 2:

v_max ..maximum speed [pulses/s]
Acc..maximum acceleration [pulses/s^2] or
Acc=v_max/Tramp..Tramp ramp time to max velocity
T_s...sampling time [s]

if (FWD==1) {
v_set=v_set+Acc*T_s; //pulses per second
if v_set>v_max v_set=v_max; }
elsif (BKW==1){
v_set=v_set-Acc*T_s;
if v_set<-v_max v_set=-v_max;}
else{          //stop moving
if v_set>0.0 {
v_set=v_set-Acc*T_s;
if v_set<0.0  v_set=0.0;
}
if v_set<0.0 {
v_set=v_set+Acc*T_s;
if v_set>0.0  v_set=0.0;
}
}

p_float=p_float + v_set*T_s; //pulses
if p_float>32767.0 p_float=p_float-65535.0; //for 16-bit position counter
else if p_float<-32768.0 p_float=p_float+65535.0;

p_int = int_16(p_float);
epsilon=p_int-p_measured; //all numbers equal int16, 32,...
y_out=Kp*float(epsilon)+v_set; //pulses per second, velocity is fed forward
y_pwm = y_out * k_pulse;//rpm , k_pulse [rpm/pulses]
y_pwm = y_pwm * kv; //volts, kv[V/rpm] from motor spec
y_pwm = y_pwm * k_pwm; //% k_pwm[%/V] - 100% equals to Vcc volts, approx.
if y_pwm>100.0 y_pwm=100.0; // max positive limit pwm DT
else if y_pwm<-100.0 < y_pwm=-100.0; //max neg limit pwm DT


EDIT 3: Industrial PID contrller zn3fd source code. The algorithm is incremental : $u(k)=u(k-1)+\Delta u(k), \;\Delta u(k)= \Delta P+ \Delta I+\Delta D$ Transfer function: $G(s) = K_p(1+\dfrac{1}{sT_i}+ \dfrac{sT_d}{1+sT_d/\alpha})$ Note $alpha$ is a filtering factor of D-component it is reccomended to be from 4 to 20, default value is 10. Bigger value means less filtering, lower value means more filter, it makes no sense to use low values.

http://www.mathworks.com/matlabcentral/fx_files/8381/1/content/help/html/STCSL.htm

// industrial PID controller, author: Bobal et al.
// parameters: Kp, Ti, Td, alpha, u_in, u_max
// inputs: Setponit, ProcesVar
// output: u

double ek, ek1, ek2, uk1, uk2, Setponit, ProcesVar;
double Ts, gamma, u_min, u_max;
double Kpu, Tu, Kp, Ti, Td, Tf, cf, ci, cd;
double q0, q1, q2, p1, p2;

//initialzation, calcualte coefficients
ek1=ek2=uk1=uk2=u=0.0;
Tf = Td/alpha;
cf = Tf/Ts;
ci = Ts/Ti;
cd = Td/Ts;
p1 = -4*cf/(1+2*cf);
p2 = (2*cf-1)/(1+2*cf);
q0 = Kp * (1 + 2*(cf+cd) + (ci/2)*(1+2*cf))/(1+2*cf);
q1 = Kp * (ci/2-4*(cf+cd))/(1+2*cf);
q2 = Kp * (cf*(2-ci) + 2*cd + ci/2 - 1)/(1+2*cf);

// ISR PID algo
ek2 = ek1;
ek1 = ek;
ek = (Setpoint - ProcesVar);
uk2 = uk1;
uk1 = u;

u = q0*ek + q1*ek1 + q2*ek2 - p1*uk1 - p2*uk2;
//limit
if   u>u_max u = u_max;
else if u<u_min u = u_min;

• sorry, I'm not sure I fully understand the pseudo code posted and how I can incorporate it in my algorithm. Will you be able to elaborate more on the 3 terms in the square brackets and why only Kp is used here? Thanks in advance for your time. – ckong80 Jun 8 '16 at 16:15
• @ckong80 I will expand my answer with some source code, for PID an PI, I will cite the author and his book. Your problem could be too much gain, or poor PID implementation with known anti-windup problems, further the D-part can be too sensitive to the noise, so my reccomandation is to try PI controller first. You should also give more detail, as you mentioned to have PID controller, but no information what are the process values and what setpoints, I am assuming that you control PWM, which outputs variable voltage to the motor, but do you have the velocity information? – Marko Buršič Jun 8 '16 at 16:45
• i am actually using only PD controller. I think I made a mistake like you said. My logic was as follows: //old logic: control = KP * (desired_angle - measured_angle) + KD * (angle_velocity_current - angle_velocity_old) / looptime; new logic: error_current = desired_angle - measured_angle control = KP * (desired_angle - measured_angle) + KD * (error_current - error_old) / looptime; error_old = error_current; Is my new logic correct or did I miss out something again? – ckong80 Jun 11 '16 at 23:54
• I am also curious on why a PI controller would be more applicable for my application. My understanding is that the integral gain is to remove steady state error while the proportionate gain is to provide damping and minimize oscillation. Without the D term, wouldn't my robot go into perpetual oscillation? – ckong80 Jun 11 '16 at 23:55
• Your post is awesome. Thanks so much for taking the time to do what you are doing. I'm not sure that I have fully understood everything that you've written.. it will probably take some time for these ideas to sink in. I thought a good starting point would be to clarify on what's fuzzy to me, one point at a time. In edit 2, v_set=v_set+Acc*T_s; //pulses per second does Acc refer to the angular acceleration? Currently with the MPU6050, I am able to get a very stable angle output using its onboard digital motion processor(dmp). However, I am not too sure how to translate that to Acc – ckong80 Jun 13 '16 at 7:23

The L298 chip is rated for a max of 2A per channel with continuous DC operation (2.5A if being controlled by PWM with 80% on and 20% off), but Pololu's 37D motors have a stall current of 5A. This may not answer your question(I don't have enough reputation to post it as a comment), but it might be a good idea to get a higher current motor driver.

• you have a very good point. Thanks for pointing this out. I have not consider this point before and I think it explains why the motor only starts moving at 50% duty cycle. I am looking at another controller from polulu that can provide upto 13A continuous current. Will try it out and provide some updates here. Thanks again for the reply. – ckong80 Jun 11 '16 at 23:28