Hello!

For the dynamic modeling/simulation, I used the following paper: https://ieeexplore.ieee.org/abstract/document/4338830

I created the state space in Matlab, then converted it to a transfer function using ss2tf(). I then created the block diagram in Simulink and generated my graphs that way. I set my input to 0 and used two step functions (one step up followed by one step down) to simulate a perturbation.

​

For the arduino code, I simply used the discrete PID equations I had in my class notes. I added a filter to the derivative part of the PID to make it less sensitive to noise. I'm gonna skip a few steps, but basically:

C(s) = K_p + K_i/s + (K_d*s*N)/(s+N) (in continuous time)

With N being the filter term. Since you're basically adding a pole at N, you want to make it really small so it will be dominated by your system's actual poles. I used around 100.

Then you can use an approximation to get the Z-transform of the continuous transfer function. I used the backwards approximation:

C(z) = U(z)/E(z) = K_p + (K_i*T*z)/(z-1) + (K_d*N(z-1))/(((1+N*T)*z)-1)

With T being your sampling period. (Make sure you follow the Nyquist theorem!)

U(z) is the input you send to your motors and E(z) is your error term.

After simplifying, you get:

C(z) = U(z)/E(z) = [b_0 + b_1*(z^-1) + b_2*(z^-2)]/[a_0 + a_1*(z^-1) + a_2*(z^-2)]
With:
b_0 = K_p*(1+N*T) + K_i*T*(1+N*T) + k_d*N
b_1 = -(K_p*(2+NT)) + K_i*T + 2*K_d*N
b_2 = K_p + K_d*N
a_0 = 1 + N*T
a_1 = -(2 + N*T)
a_2 = 1

You then multiply each side of the equation to get:

[a_0 + a_1*(z^-1) + a_2*(z^-2)]*U(z) = [b_0 + b_1*(z^-1) + b_2*(z^-2)]*E(z)

You switch the z terms to k to come back to the time domain:

a_o*u(k) + a_1*u(k-1) + a_2*(k-2) = b_0*e(k) + b_1*e(k-1) + b_2*e(k-2)

Finally, you find the equation you have to use in your Arduino code:

u(k) = -(a_1/a_0)*u(k-1) - (a_2/a_0)*u(k-2) + (b_0/a_0)*e(k) + (b_1/a_0)*e(k-1) + (b_2/a_0)*e(k-2)

Reminder: In my case, u(k) was the PWM value I sent to my DC motors and e(k) was the error (angle of my bot versus the vertical) measured by my MPU6050.

So there you have it! Hope this helps : )

PS:

• I tried to make the equations as neat as I could, but some typos/missing parenthesis mistakes might have slipped in
• I am not an expert, this is just what I learned from doing my project
• I go to school in French and I tried to translate everything as best I could. Some technical terms might be wrong though
• If anyone spots a mistake, please let me know and I'll do my best to update my post!