How to design a position control in C using Matlab

I am working on designing a micro mouse that solves a maze. The objective is to reach the centre of the maze. The mouse needs to move one block distance at a time, which is 18 cm. Currently, I am using a position control which calculates the error in distance and converts that into PWM value. So, as the error gets lower, the PWM decreases. Below is the code for it:

if( distanceRight < 60 || distanceLeft < 60)    // Encoder Pulses from the left and the right wheel
        {
            //Finding the error and converting into fraction values to be used as PWM 
            errorRight = (60.0 - distanceRight)/60; 
            errorLeft = (60.0 - distanceLeft)/60;

              PWM_RightWheel = (errorRight);  
              PWM_LeftWheel = (errorLeft);            

              distanceRight = abs(encoder1.getPulses());
              distanceLeft = abs(encoder2.getPulses());
          }

But, here is the problem. Sometimes the mouse overshoots due to its inertia. The encoder that I am using to measure the distance has a max of 48 state changes for one revolution. Considering the radius of the wheel, this gives me the ability to measure 0.25cm distances accurately.

What I need to do is to design a PID or a PD controller for it. How can I go about that? I have generated a PWM vs Encoder Pulse Count graph (pic of the graph is attached) for the wheels for different duty cycles. How can I use it to find the transfer function for the wheel and then implement it in C code?

Answer 1

[Santhana Raj]

For PID equation, you have to find the previous error values also. Foe example, for integral action, sum of all previous errors multiplied by the integral const and for differential action, difference between the last 2 successive error multiplied by the diff const has to be added to the errorRight and errorLeft.

Chk google for discrete PID control equation. You will get the idea.