pidcontrol.c

实现了dsPIC30f6012a通过SPC3与PLC通信

#include "Headfiles.h"

/*
void PID_Arithmetic(int kp_parameter, int ki_parameter, int kd_parameter,
                    int *error_sum, int error, int *offset,
                    int *pid_output)
{
    long int temp_lint;
    long int pid_proportional, pid_integral, pid_derivative;
    
    offset[0] = error;
    
    if ((error > 400) || (error < -400))                      
    {
        *error_sum = 0;
        if (error > 300)
        {
            *pid_output = 2000;//1500;
        }
        else
        {
            *pid_output = -2000;//1500;
        }
    }
    else if ((error > 200) || (error < -200))
    {
        *error_sum = 0;
        if (error > 200)
        {
            *pid_output = 500;//400;
        }
        else
        {
            *pid_output = -500;//400;
        }
    }
    else
    {
        temp_lint = offset[0];
        pid_proportional = (temp_lint * kp_parameter) >> 5;
        
        if ((temp_lint >= -10) && (temp_lint <= 10))
        {
            *error_sum += temp_lint;
        }
        else
        {
            *error_sum = 0;
        }
        if ((*error_sum >= 20000) || (*error_sum <= -20000))
        {
            if (*error_sum >= 20000)
            {
                *error_sum = 20000;
            }
            else
            {
                *error_sum = -20000;
            }
        }

        temp_lint = *error_sum;
        pid_integral = (temp_lint * ki_parameter) >> 10;
        
        if ((pid_integral >= 300) || (pid_integral <= -300))
        {
            if (pid_integral >= 300)
            {
                pid_integral = 300;
            }
            else
            {
                pid_integral = -300;
            }
        }
        //test


        temp_lint = offset[0] - offset[1];
        pid_derivative = (temp_lint * kd_parameter) >> 5;
        
        *pid_output = (pid_proportional + pid_integral + pid_derivative) >> 4;
       offset[4] = offset[3];
       offset[3] = offset[2];
       offset[2] = offset[1];
       offset[1] = offset[0];
    }
    return;
}*/

void PID_Arithmetic(int error, unsigned channel)
{
    long int temp_lint;
    long int pid_proportional, pid_integral, pid_derivative;
    
    Offset[channel][0] = error;
    
    if ((error > 400) || (error < -400))                      
    {
        PID_Error_Sum[channel] = 0;
        if (error > 400)
        {
            PID[channel] = 32000;
        }
        else
        {
            PID[channel] = -32000;
        }
    }
    else if ((error > 200) || (error < -200))
    {
        PID_Error_Sum[channel] = 0;
        if (error > 200)
        {
            PID[channel] = 8000;
        }
        else
        {
            PID[channel] = -8000;
        }
    }
    else
    {
        temp_lint = error;
        if (channel == 0)
        {
            pid_proportional = (temp_lint * Control_Parameter[0]) >> 5;
            PID_Proportional[0] = pid_proportional;
        }
        else
        {
            pid_proportional = (temp_lint * Control_Parameter[7]) >> 5;
            PID_Proportional[1] = pid_proportional;
        }
        
        if ((temp_lint >= -10) && (temp_lint <= 10))
        {
            PID_Error_Sum[channel] += temp_lint;
        }
        else
        {
            PID_Error_Sum[channel] = 0;
        }
        if ((PID_Error_Sum[channel] >= 20000) || (PID_Error_Sum[channel] <= -20000))
        {
            if (PID_Error_Sum[channel] >= 20000)
            {
                PID_Error_Sum[channel] = 20000;
            }
            else
            {
                PID_Error_Sum[channel] = -20000;
            }
        }

        temp_lint = PID_Error_Sum[channel];
        if (channel == 0)
        {
            pid_integral = (temp_lint * Control_Parameter[1]) >> 10;
            PID_Integral[0] = pid_integral;
        }
        else
        {
            pid_integral = (temp_lint * Control_Parameter[8]) >> 10;
            PID_Integral[1] = pid_integral;
        }
        
        if ((pid_integral >= 300) || (pid_integral <= -300))
        {
            if (pid_integral >= 300)
            {
                pid_integral = 300;
            }
            else
            {
                pid_integral = -300;
            }
        }

        temp_lint = Offset[channel][0] - Offset[channel][1];
        if (channel == 0)
        {
            pid_derivative = (temp_lint * Control_Parameter[2]) >> 5;
            PID_Derivative[0] = pid_derivative;
        }
        else
        {
            pid_derivative = (temp_lint * Control_Parameter[9]) >> 5;
            PID_Derivative[1] = pid_derivative;
        }
        
        PID[channel] = pid_proportional + pid_integral + pid_derivative;
        Offset[channel][4] = Offset[channel][3];
        Offset[channel][3] = Offset[channel][2];
        Offset[channel][2] = Offset[channel][1];
        Offset[channel][1] = Offset[channel][0];
    }
    return;
}

void Force_Feedback(int force_limit, int force_norm, int k_practice,
                    int k_simulation, int error, int force, int *delta_error)
{
    int delta_force;
    int simulation;
    int temp;

    simulation = ((long int)error * k_simulation) >> 8;
    
    delta_force = force - force_norm;

    if ((delta_force > force_limit) || (delta_force < -force_limit))
    {
        if (delta_force > force_limit)
        {
            *delta_error = force_limit - delta_force;
        }
        else
        {
            *delta_error = -(force_limit + delta_force);
        }
    }
    else
    {
        *delta_error = 0;
    }
    *delta_error = ((long int)*delta_error * k_practice) >> 8;
    
    if (*delta_error > 0)
    {
        temp = 2 * (*delta_error);
    }
    else
    {
        temp = -2 * (*delta_error);
    }
    if ((simulation >= temp) || (simulation <= -temp))
    {
        if (simulation >= temp)
        {
            simulation = temp;
        }
        else
        {
            simulation = -temp;
        }
    }
    else
    {
    }
    *delta_error = (*delta_error - simulation) << 6;
    
    if ((*delta_error >= 6000) || (*delta_error <= -6000))
    {
        if (*delta_error >= 6000)
        {
            *delta_error = 6000;
        }
        else
        {
            *delta_error = -6000;
        }
    }
    else if ((*delta_error >= 600) || (*delta_error <= -600))
    {
        if (*delta_error >= 600)
        {
            *delta_error = 600;
        }
        else
        {
            *delta_error = -600;
        }
    }
    else
    {
    }
    
    if ((*delta_error <= 320) && (*delta_error >= -320))
    {
        *delta_error = 0;
    }
}


void PID_Control(void)
{   
    unsigned char i;
    int force_feedback_error[2];
    
    if(Function_Ctrl == 1)
    {
        Force_Feedback(Control_Parameter[3], Control_Parameter[4], Control_Parameter[5],
                       Control_Parameter[6], L_Cylinder[0] - L_Set[0],
                       ((long int)P1_Cylinder[0] * 367 >> 8) - P2_Cylinder[0],
                       &force_feedback_error[0]);
        Force_Feedback(Control_Parameter[10], Control_Parameter[11], Control_Parameter[12],
                       Control_Parameter[13], L_Cylinder[1] - L_Set[1],
                       ((long int)P1_Cylinder[1] * 367 >> 8) - P2_Cylinder[1],
                       &force_feedback_error[1]);
        
        for (i = 0; i < 2; i++)
        {
            if ((force_feedback_error[i] == 0) && (P_Force_Sum[i] > -20) &&
                (P_Force_Sum[i] < 20))
            {
                P_Force_Sum[i] = 0;
            }
        }
        P_Force_Sum[0] += force_feedback_error[0];
        P_Force_Sum[1] += force_feedback_error[1];

        for (i = 0; i < 2; i++)
        {
            if (P_Force_Sum[i] >= 1000)
            {
                P_Force_Sum[i] = 1000;
            }
            else if (P_Force_Sum[i] <= -1000)
            {
                P_Force_Sum[i] = -1000;
            }
        }
    }
    else
    {
        P_Force_Sum[0] = 0;
        P_Force_Sum[1] = 0;
    }    
    
    PID_Arithmetic(L_Set[0] - L_Cylinder[0] + P_Force_Sum[0], 0);
    /*PID_Arithmetic(Control_Parameter[0], Control_Parameter[1], Control_Parameter[2],
                   &PID_Error_Sum[0], L_Set[0] - L_Cylinder[0] + P_Force_Sum[0],
                   Offset[0], &PID[0]);*/
    if (Function_Ctrl == 2)
    {
        Zero_Study_Self(0, PID[0]);
        PID_Error_Sum[0] = 0;
    }
    
//    PID_Arithmetic(L_Set[1] - L_Cylinder[1] + P_Force_Sum[1], 1);
    /*PID_Arithmetic(Control_Parameter[7], Control_Parameter[8], Control_Parameter[9],
                   &PID_Error_Sum[1], L_Set[1] - L_Cylinder[1] + P_Force_Sum[1],
                   Offset[1], &PID[1]);*/
    if (Function_Ctrl == 3)
    {
        Zero_Study_Self(1, PID[1]);
        PID_Error_Sum[1] = 0;
    }
}