Revision history [back]

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\n\n", kinematics_measured.linear.x, kinematics_measured.linear.y, kinematics_measured.angular.z);

       usleep(1000);    // 1khz for PID to work properly
}

And the functions:

uint16_t computeDuty(float v)
{
    uint16_t p;

    if(v>1.02)
        v=1.02; // Maximum measured speed with duty=1023

    return ((uint16_t) ((1023/1.02)*v));
}


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

    // 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;
    prev_error = error;

    pid = Pout+Iout+Dout;

    if(pid>0)
        if(pid>1.02)    //Maximum measured speed
            return 1.02;
        else
            return pid;
    else
        if(pid<-1.02)
            return (-1.02);
        else
            return pid;
}

float abs(float v)
{
    if(v<0)
        return ((-1)*v);
    else
        return v;
}

What I am missing is how should I limit the integral part of the pid? How should I determine the limits? Are they also the maximum values measured?

Thanks for the help.