PID on DC motor turning on both directions

Question

I have a skid steer robot where I have four individual DC motors with an H bridge so I can control speed and direction. There are also four quadrature encoders with 6000 ticks per revolution. The motors are 12v and I am controlling them with a 10bit PWM (0 to 1023). First test I did was to raise all wheels and test the minimum value where the wheels start moving, which is around 500.

I am able to measure the current speed of the wheels correctly by using a counter where the quadrature encoder set it to count up or down. Therefore, by reading the current count every 50ms (20Hz), I can measure the current speed:

       encoders_curr.FR = COUNTER_mReadReg(ENC0_BASEADDR, COUNTER_S00_AXI_SLV_REG1_OFFSET);
       encoders_curr.FL = COUNTER_mReadReg(ENC1_BASEADDR, COUNTER_S00_AXI_SLV_REG1_OFFSET);
       encoders_curr.RR = COUNTER_mReadReg(ENC2_BASEADDR, COUNTER_S00_AXI_SLV_REG1_OFFSET);
       encoders_curr.RL = COUNTER_mReadReg(ENC3_BASEADDR, COUNTER_S00_AXI_SLV_REG1_OFFSET);

       encoders_diff.FR = encoders_curr.FR - encoders_prev.FR;
       encoders_diff.FL = encoders_curr.FL - encoders_prev.FL;
       encoders_diff.RL = encoders_curr.RL - encoders_prev.RL;
       encoders_diff.RR = encoders_curr.RR - encoders_prev.RR;

       encoders_prev = encoders_curr;

       speed_measured.FR = (2*M_PI*WHEEL_RADIUS*encoders_diff.FR) / (0.05*TICKS);
       speed_measured.FL = (2*M_PI*WHEEL_RADIUS*encoders_diff.FL) / (0.05*TICKS);
       speed_measured.RR = (2*M_PI*WHEEL_RADIUS*encoders_diff.RR) / (0.05*TICKS);
       speed_measured.RL = (2*M_PI*WHEEL_RADIUS*encoders_diff.RL) / (0.05*TICKS);

Which I convert to X, Y and Z with:

       // Inverse Kinematics: http://robotsforroboticists.com/drive-kinematics/
       kinematics_measured.linear.x = (speed_measured.FR+speed_measured.FL+speed_measured.RR+speed_measured.RL)*(WHEEL_RADIUS/4);
       kinematics_measured.linear.y = (-speed_measured.FR+speed_measured.FL+speed_measured.RR-speed_measured.RL)*(WHEEL_RADIUS/4);
       kinematics_measured.angular.z = (-speed_measured.FR+speed_measured.FL-speed_measured.RR+speed_measured.RL)*(WHEEL_RADIUS/(4*(WHEEL_SEPARATION_WIDTH+WHEEL_SEPARATION_LENGTH)));

Now for the control part, to set the individual velocities of each of the four wheels, I am using these equations, as my input is linear x, linear Y and angular Z speeds (rad/seg):

       // Forward kinematics: http://robotsforroboticists.com/drive-kinematics/
       speed_desired.FL = (1/WHEEL_RADIUS)*(kinematics_desired.linear.x-kinematics_desired.linear.y-kinematics_desired.angular.z*(WHEEL_SEPARATION_LENGTH+WHEEL_SEPARATION_WIDTH));
       speed_desired.FR = (1/WHEEL_RADIUS)*(kinematics_desired.linear.x+kinematics_desired.linear.y+kinematics_desired.angular.z*(WHEEL_SEPARATION_LENGTH+WHEEL_SEPARATION_WIDTH));
       speed_desired.RL = (1/WHEEL_RADIUS)*(kinematics_desired.linear.x+kinematics_desired.linear.y-kinematics_desired.angular.z*(WHEEL_SEPARATION_LENGTH+WHEEL_SEPARATION_WIDTH));
       speed_desired.RR = (1/WHEEL_RADIUS)*(kinematics_desired.linear.x-kinematics_desired.linear.y+kinematics_desired.angular.z*(WHEEL_SEPARATION_LENGTH+WHEEL_SEPARATION_WIDTH));

The PID function that I have implemented is the following:

float integral=0.0, prev_error=0.0;
#define dt  0.05    // in seconds
#define Kp  1
#define Ki  0
#define Kd  0
float pid(float desired, float measured)
{
    if(measured<0)
        measured = (-1)*measured;

    // Calculate error
    float error = desired-measured;

    // Proportional term
    float Pout = Kp*error;

    // Integral term
    integral += error*dt;
    float Iout = Ki*integral;

    // Derivate term
    float derivate = (error-prev_error)/dt;
    float Dout = Kd*derivate;

    return (Pout+Iout+Dout);
}

So, after measuring the current speed, I can call it to obtain the speed I need to set the duty of each motor.

My questions:

1. The sign of the velocity indicates the direction of rotation (clockwise=forward, counterclockwise=backwards). Do I need to pass to the pid function the desired velocity as a signed or unsigned value?
2. What should I do with velocity values that would mean a lower pwm so the motor will not spin?
3. To convert from linear velocity that the pid will dictate to duty cycle, I am calculating the equation that passes over two points. For this, I measured the speeds at the minimum and maximum duties where the wheels rotation (500 and 1023) giving (0.81 and 1.03). I use these numbers to get the equation: p = 511 + (uint16_t)(((1023-511)/(1.02-0.8))*(v-0.8));. Should I consider that the speed can be negative to compute a "negative" duty which will later represent only the direction to set for the motors? If so, it means that I have to consider that the two points for the equation are (-1023, -1.03) and (1023, 1.03)?
4. As I have four motors, they all show different velocities. So, to simplify the process, even thought there will be 4 PIDs, I got for the lowest duty cycle the maximum velocity value of all motors and for the highest duty cycle the lowest velocity of all motors. Would that be correct?
5. Do I need to include the speed limits for the PID?
6. Considering the signs, won't be a problem if the measured speed is higher than the desired, the pid would compute a negative value and the fact that the rotation switches directions might cause an issue, or not?

Thank you for the help.

Answer 1

Q1

Do I need to pass to the pid function the desired velocity as a signed or unsigned value?

Definitely, you shall deal with signed velocities. The PID will take care of all the signs down the operations' chain, providing positive and or negative rotational velocities.

Q2

What should I do with velocity values that would mean a lower pwm so the motor will not spin?

The integral part of the PID will be responsible for that. Anyway, see the note on the anti-windup.

Q3

Should I consider that the speed can be negative to compute a "negative" duty ...?

Sure! As per Q1. Anyway, it's not clear to me why you're doing the conversion from velocity to duty. The PID will find its way to it. Let the PID do its job.

Q4

Would that be correct?

I don't think so. You're adding up knowledge on top of the task that is not required, making things overcomplicated. Just let the PID do its job based on the measurements and it will work. If the measurements are reliable, the 4 PID will make the 4 wheels spin at the same speed (after transients), regardless of their constructive differences.

The task of attaining the same speed on the 4 different wheels is thus doable at the steady-state. However, that doesn't mean that the robot will be reaching your target location or tightly following your desired path as controlling velocity does not guarantee that you can compensate for different position errors that will accumulate during transitions. To get around this, you may consider applying a hierarchical approach with an outer controller that will close the loop on the position (e.g. odometry, SLAM).

Q5

Do I need to include the speed limits for the PID?

Although it's not strictly required, it may be beneficial. Be careful as the speed limits (a.k.a. actuation bounds) need to be handled properly by means of the anti-windup.

Q6

... won't be a problem if the measured speed is higher than the desired ...?

Nope, the PID is apt to the task of controlling around 0 error, thus the signs are correctly handled.

Integral anti-windup

The PID needs to be aware that the process under control does show actuation limits. To this end, one can apply the standard mechanism of anti-windup to prevent the integral term from overcharging when the system working point is beyond those bounds. The net result is that the anti-windup purposely avoids unwanted oscillations.

See this answer for more information on the anti-windup.