Skid steer robot PID control with DC motors

asked 2020-08-01 10:04:49 -0600

Ariel
65 ●23 ●31 ●36

updated 2020-08-01 10:42:11 -0600

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 (geometry_msgs/Twist) 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:

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?
What should I do with velocity values that would mean a ...

(more)

edit retag flag offensive close merge delete

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2020-08-01 15:48:30 -0600

billy
1850 ●24 ●87 ●47

updated 2020-08-01 15:52:23 -0600

Lots of questions: but first a couple comments on your code.

You never update value to previous_error so it will always be zero.
I'm not sure why you take the inverse of measured when negative. You do not want to do that...unless your robot can only go forward.
You want to cap the output of the PID algo with limits(positive and negative) to allow you to set max torque or speed in the motors. This is important to have when you're tuning your robot initially and then later when you have encoder failures some day so robot has a limited amount of fit it can throw.
You should put a cap on your integral term(positive and negative) to prevent what is called integral windup. Google it to see why.
I cannot say it will not work, but I have no experience with PIDs loops on DC motors updating at anything less than 1000Hz. Your PID loop should operate above the well above bandwidth of your motor/drive combo. On large induction motors 20Hz seems fine, but on a small robot with DC motors I can't say. I'm sure the world-interweb will tell you if 20Hz can work. You'll have to tune the PID to be fairly un-responsive at 20Hz.

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?

Yes. Use the PID algo as the full control without splitting out the forward/reverse to some other code. At the point that you send PID output to PWM controller, you want to check if value is positive or negative and set the direction on the H-Bridge accordingly and use ABS(PID) to the H-Bridge.

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

You should not have to worry about this. PID algo will do what is required to make motor move when it should and not when it should not.

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)?

I understand you're trying to calculate your PID coefficients, but unless this is a school assignment do not bother. Code the algo and set your coefficients through manual tuning. The positive and negative ... (more)

edit flag offensive delete link

add a comment

answered 2020-08-03 05:36:23 -0600

Ariel
65 ●23 ●31 ●36

Thank you @billy for the help.

I removed all the checking whether it is negative or not and send the measured values as they are to the pid function. The return of it, I check for maximum values. To be on the safe side, this is the maximum I measured, right?

Here is the "clean" code:

   while(1)
    {
       /***** Desired (input) *****/
       // 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));

       /***** Measured (output) *****/
       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);

       /***** PID *****/
       speed_pid.FR = pid(speed_desired.FR, speed_measured.FR);
       speed_pid.FL = pid(speed_desired.FL, speed_measured.FL);
       speed_pid.RR = pid(speed_desired.RR, speed_measured.RR);
       speed_pid.RL = pid(speed_desired.RL, speed_measured.RL);

       /***** Convert speed to duty cycle *****/
       duties.FL = computeDuty(abs(speed_pid.FL));
       duties.FR = computeDuty(abs(speed_pid.FR));
       duties.RL = computeDuty(abs(speed_pid.RL));
       duties.RR = computeDuty(abs(speed_pid.RR));

       /***** Set PID's speeds *****/
       if(speed_pid.FL>=0)
           PWM_mWriteReg(PWM_BASEADDR, FLM, ((CLOCKWISE<<11) | (ENABLE<<10) | duties.FL));
       else
           PWM_mWriteReg(PWM_BASEADDR, FLM, ((COUNTERCLOCKWISE<<11) | (ENABLE<<10) | duties.FL));

       if(speed_pid.FR>=0)
           PWM_mWriteReg(PWM_BASEADDR, FRM, ((CLOCKWISE<<11) | (ENABLE<<10) | duties.FR));
       else
           PWM_mWriteReg(PWM_BASEADDR, FRM, ((COUNTERCLOCKWISE<<11) | (ENABLE<<10) | duties.FR));

       if(speed_pid.RL>=0)
           PWM_mWriteReg(PWM_BASEADDR, RLM, ((CLOCKWISE<<11) | (ENABLE<<10) | duties.RL));
       else
           PWM_mWriteReg(PWM_BASEADDR, RLM, ((COUNTERCLOCKWISE<<11) | (ENABLE<<10) | duties.RL));

       if(speed_pid.RR>=0)
           PWM_mWriteReg(PWM_BASEADDR, RRM, ((CLOCKWISE<<11) | (ENABLE<<10) | duties.RR));
       else
           PWM_mWriteReg(PWM_BASEADDR, RRM, ((COUNTERCLOCKWISE<<11) | (ENABLE<<10) | duties.RR));

       printf("FL: desired-measured = %.3f - %.3f = error = %.3f --> set = %.3f (duty: %d)\n", speed_desired.FL, speed_measured.FL, speed_desired.FL - speed_measured.FL, speed_pid.FL, duties.FL);
       printf("FR: desired-measured = %.3f - %.3f = error = %.3f --> set = %.3f (duty: %d)\n", speed_desired.FR, speed_measured.FR, speed_desired.FR - speed_measured.FR, speed_pid.FR, duties.FR);
       printf("RL: desired-measured = %.3f - %.3f = error = %.3f --> set = %.3f (duty: %d)\n", speed_desired.RL, speed_measured.RL, speed_desired.RL - speed_measured.RL, speed_pid.RL, duties.RL);
       printf("RR: desired-measured = %.3f - %.3f = error = %.3f --> set = %.3f (duty: %d)\n", speed_desired.RR, speed_measured.RR, speed_desired.RR - speed_measured.RR, speed_pid.RR, duties.RR);

       // 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)));

       printf("X: %.4f -Y: %.4f - Z: %.4f ...

(more)

edit flag offensive delete link

Comments

Regarding the on the PID and integrators, they are different purpose.

The PID limit is limit the torque or speed getting too high. As example, I had an outdoor robot get away from me and it took off and ran into the lawn mower and flip it up in the air like a bullfighter getting flung by a bull. Both the lawnmower and robot survived, but what if that had been a person or animal getting hit and injured? Another small robot indoors absolutely lost it's mind and ran into the wall faster than I thought it could go. Build in a limit unless you know the robot is physically incapable of doing something too hard or too fast to cause an issue. Then make sure the code records anytime that limit is hit so you're aware in case that limit is getting hit during normal operation and ...

(more)

billy ( 2020-08-04 01:19:10 -0600 )edit

add a comment

Skid steer robot PID control with DC motors

2 Answers

Comments

Question Tools

Stats

Related questions

Skid steer robot PID control with DC motors edit

2 Answers

Comments

Question Tools

Stats

Related questions

Skid steer robot PID control with DC motors