void pid_update(pid_type* pid){
    pid->error = pid->in - pid->ref;
    pid->proportional = pid->Kp * (pid->error - pid->error_old1);
    pid->integral = pid->Ki * pid->error;
    pid->derivative = pid->Kd * (pid->in - 2*pid->in_old1 + pid->in_old2);

    if(pid->in >= 5 || pid->in <= -5){
        pid->integral = 0.1 * pid->integral;
    }
    pid->out = pid->out_old + pid->proportional + pid->derivative + pid->integral;
    pid->in_old2 = pid->in_old1;
    pid->in_old1 = pid->in;
    pid->error_old1 = pid->error;
    pid->out_old = pid->out;
}