mirror of
https://gitee.com/Lyon1998/pikapython.git
synced 2025-01-29 17:22:56 +08:00
593 lines
19 KiB
C
593 lines
19 KiB
C
#include "fuzzy_PID.h"
|
|
|
|
struct fuzzy* fuzzy_init(unsigned int input_num, unsigned int output_num) {
|
|
struct fuzzy* fuzzy_struct =
|
|
(struct fuzzy*)pika_platform_malloc(sizeof(struct fuzzy));
|
|
fuzzy_struct->input_num = input_num;
|
|
fuzzy_struct->output_num = output_num;
|
|
fuzzy_struct->mf_type = (unsigned int*)pika_platform_malloc(
|
|
(input_num + output_num) * sizeof(unsigned int));
|
|
#ifdef fuzzy_pid_rule_base_deep_copy
|
|
fuzzy_struct->mf_params =
|
|
(int*)pika_platform_malloc(4 * qf_default * sizeof(int));
|
|
fuzzy_struct->rule_base = (int*)pika_platform_malloc(
|
|
output_num * qf_default * qf_default * sizeof(int));
|
|
#endif
|
|
fuzzy_struct->output =
|
|
(float*)pika_platform_malloc(output_num * sizeof(float));
|
|
return fuzzy_struct;
|
|
}
|
|
|
|
void delete_fuzzy(struct fuzzy* fuzzy_struct) {
|
|
pika_platform_free(fuzzy_struct->mf_type);
|
|
pika_platform_free(fuzzy_struct->output);
|
|
pika_platform_free(fuzzy_struct);
|
|
}
|
|
|
|
void fuzzy_params_init(struct fuzzy* fuzzy_struct,
|
|
unsigned int mf_type,
|
|
unsigned int fo_type,
|
|
unsigned int df_type,
|
|
int mf_params[],
|
|
int rule_base[][qf_default]) {
|
|
for (unsigned int i = 0;
|
|
i < fuzzy_struct->input_num + fuzzy_struct->output_num; ++i) {
|
|
fuzzy_struct->mf_type[i] = mf_type;
|
|
}
|
|
|
|
for (unsigned int i = 0; i < fuzzy_struct->output_num; ++i) {
|
|
fuzzy_struct->output[i] = 0;
|
|
}
|
|
|
|
#ifdef fuzzy_pid_rule_base_deep_copy
|
|
for (unsigned int j = 0; j < 4 * qf_default; ++j) {
|
|
fuzzy_struct->mf_params[j] = mf_params[j];
|
|
}
|
|
|
|
for (unsigned int k = 0; k < fuzzy_struct->output_num * qf_default; ++k) {
|
|
for (unsigned int i = 0; i < qf_default; ++i) {
|
|
fuzzy_struct->rule_base[k * 7 + i] = rule_base[k][i];
|
|
}
|
|
}
|
|
#else
|
|
fuzzy_struct->mf_params = mf_params;
|
|
fuzzy_struct->rule_base = (int*)rule_base;
|
|
#endif
|
|
|
|
fuzzy_struct->fo_type = fo_type;
|
|
fuzzy_struct->df_type = df_type;
|
|
}
|
|
|
|
#define inverse(parameter) 1.0f / (float)parameter
|
|
|
|
// Gaussian membership function
|
|
float gaussmf(float x, float sigma, float c) {
|
|
return expf(-powf(((x - c) / sigma), 2.0f));
|
|
}
|
|
|
|
// Generalized bell-shaped membership function
|
|
float gbellmf(float x, float a, float b, float c) {
|
|
return inverse(1.0f + powf(fabsf((x - c) / a), 2.0f * b));
|
|
}
|
|
|
|
// Sigmoidal membership function
|
|
float sigmf(float x, float a, float c) {
|
|
return inverse(1.0f + expf(a * (c - x)));
|
|
}
|
|
|
|
// Trapezoidal membership function
|
|
float trapmf(float x, float a, float b, float c, float d) {
|
|
if (x >= a && x < b)
|
|
return (x - a) / (b - a);
|
|
else if (x >= b && x < c)
|
|
return 1.0f;
|
|
else if (x >= c && x <= d)
|
|
return (d - x) / (d - c);
|
|
else
|
|
return 0.0f;
|
|
}
|
|
|
|
// Triangular membership function
|
|
float trimf(float x, float a, float b, float c) {
|
|
return trapmf(x, a, b, b, c);
|
|
}
|
|
|
|
// Z-shaped membership function
|
|
float zmf(float x, float a, float b) {
|
|
if (x <= a)
|
|
return 1.0f;
|
|
else if (x >= a && x <= (a + b) / 2.0f)
|
|
return 1.0f - 2.0f * powf((x - a) / (b - a), 2.0f);
|
|
else if (x >= (a + b) / 2.0f && x < b)
|
|
return 2.0f * powf((x - b) / (b - a), 2.0f);
|
|
else
|
|
return 0;
|
|
}
|
|
|
|
// Membership function
|
|
float mf(float x, unsigned int mf_type, int* params) {
|
|
switch (mf_type) {
|
|
case 0:
|
|
return gaussmf(x, params[0], params[1]);
|
|
case 1:
|
|
return gbellmf(x, params[0], params[1], params[2]);
|
|
case 2:
|
|
return sigmf(x, params[0], params[2]);
|
|
case 3:
|
|
return trapmf(x, params[0], params[1], params[2], params[3]);
|
|
case 5:
|
|
return zmf(x, params[0], params[1]);
|
|
default: // set triangular as default membership function
|
|
return trimf(x, params[0], params[1], params[2]);
|
|
}
|
|
}
|
|
|
|
// Union operator
|
|
float or (float a, float b, unsigned int type) {
|
|
if (type == 1) { // algebraic sum
|
|
return a + b - a * b;
|
|
} else if (type == 2) { // bounded sum
|
|
return fminf(1, a + b);
|
|
} else { // fuzzy union
|
|
return fmaxf(a, b);
|
|
}
|
|
}
|
|
|
|
// Intersection operator
|
|
float and (float a, float b, unsigned int type) {
|
|
if (type == 1) { // algebraic product
|
|
return a * b;
|
|
} else if (type == 2) { // bounded product
|
|
return fmaxf(0, a + b - 1);
|
|
} else { // fuzzy intersection
|
|
return fminf(a, b);
|
|
}
|
|
}
|
|
|
|
// Equilibrium operator
|
|
float equilibrium(float a, float b, float params) {
|
|
return powf(a * b, 1 - params) * powf(1 - (1 - a) * (1 - b), params);
|
|
}
|
|
|
|
// Fuzzy operator
|
|
float fo(float a, float b, unsigned int type) {
|
|
if (type < 3) {
|
|
return and(a, b, type);
|
|
} else if (type < 6) {
|
|
return or (a, b, type - 3);
|
|
} else {
|
|
return equilibrium(a, b, 0.5f);
|
|
}
|
|
}
|
|
|
|
// Mean of centers defuzzifier, only for two input multiple index
|
|
void moc(const float* joint_membership,
|
|
const unsigned int* index,
|
|
const unsigned int* count,
|
|
struct fuzzy* fuzzy_struct) {
|
|
float denominator_count = 0;
|
|
float numerator_count[fuzzy_struct->output_num];
|
|
for (unsigned int l = 0; l < fuzzy_struct->output_num; ++l) {
|
|
numerator_count[l] = 0;
|
|
}
|
|
|
|
for (int i = 0; i < count[0]; ++i) {
|
|
for (int j = 0; j < count[1]; ++j) {
|
|
denominator_count += joint_membership[i * count[1] + j];
|
|
}
|
|
}
|
|
|
|
for (unsigned int k = 0; k < fuzzy_struct->output_num; ++k) {
|
|
for (unsigned int i = 0; i < count[0]; ++i) {
|
|
for (unsigned int j = 0; j < count[1]; ++j) {
|
|
numerator_count[k] +=
|
|
joint_membership[i * count[1] + j] *
|
|
fuzzy_struct->rule_base[k * qf_default * qf_default +
|
|
index[i] * qf_default +
|
|
index[count[0] + j]];
|
|
}
|
|
}
|
|
}
|
|
|
|
#ifdef fuzzy_pid_debug_print
|
|
pika_platform_printf("output:\n");
|
|
#endif
|
|
for (unsigned int l = 0; l < fuzzy_struct->output_num; ++l) {
|
|
fuzzy_struct->output[l] = numerator_count[l] / denominator_count;
|
|
#ifdef fuzzy_pid_debug_print
|
|
pika_platform_printf("%f,%f,%f\n", numerator_count[l],
|
|
denominator_count, fuzzy_struct->index[l]);
|
|
#endif
|
|
}
|
|
}
|
|
|
|
// Defuzzifier
|
|
void df(const float* joint_membership,
|
|
const unsigned int* output,
|
|
const unsigned int* count,
|
|
struct fuzzy* fuzzy_struct,
|
|
int df_type) {
|
|
if (df_type == 0)
|
|
moc(joint_membership, output, count, fuzzy_struct);
|
|
else {
|
|
pika_platform_printf("Waring: No such of defuzzifier!\n");
|
|
moc(joint_membership, output, count, fuzzy_struct);
|
|
}
|
|
}
|
|
|
|
void fuzzy_control(float e, float de, struct fuzzy* fuzzy_struct) {
|
|
float membership[qf_default * 2]; // Store membership
|
|
unsigned int index[qf_default * 2]; // Store the index of each membership
|
|
unsigned int count[2] = {0, 0};
|
|
|
|
{
|
|
int j = 0;
|
|
for (int i = 0; i < qf_default; ++i) {
|
|
float temp = mf(e, fuzzy_struct->mf_type[0],
|
|
fuzzy_struct->mf_params + 4 * i);
|
|
if (temp > 1e-4) {
|
|
membership[j] = temp;
|
|
index[j++] = i;
|
|
}
|
|
}
|
|
|
|
count[0] = j;
|
|
|
|
for (int i = 0; i < qf_default; ++i) {
|
|
float temp = mf(de, fuzzy_struct->mf_type[1],
|
|
fuzzy_struct->mf_params + 4 * i);
|
|
if (temp > 1e-4) {
|
|
membership[j] = temp;
|
|
index[j++] = i;
|
|
}
|
|
}
|
|
|
|
count[1] = j - count[0];
|
|
}
|
|
|
|
#ifdef fuzzy_pid_debug_print
|
|
pika_platform_printf("membership:\n");
|
|
for (unsigned int k = 0; k < j; ++k) {
|
|
pika_platform_printf("%f\n", membership[k]);
|
|
}
|
|
|
|
pika_platform_printf("index:\n");
|
|
for (unsigned int k = 0; k < j; ++k) {
|
|
pika_platform_printf("%d\n", index[k]);
|
|
}
|
|
|
|
pika_platform_printf("count:\n");
|
|
for (unsigned int k = 0; k < 2; ++k) {
|
|
pika_platform_printf("%d\n", count[k]);
|
|
}
|
|
#endif
|
|
|
|
if (count[0] == 0 || count[1] == 0) {
|
|
for (unsigned int l = 0; l < fuzzy_struct->output_num; ++l) {
|
|
fuzzy_struct->output[l] = 0;
|
|
}
|
|
return;
|
|
}
|
|
|
|
// Joint membership
|
|
float joint_membership[count[0] * count[1]];
|
|
|
|
for (int i = 0; i < count[0]; ++i) {
|
|
for (int j = 0; j < count[1]; ++j) {
|
|
joint_membership[i * count[1] + j] = fo(
|
|
membership[i], membership[count[0] + j], fuzzy_struct->fo_type);
|
|
}
|
|
}
|
|
|
|
df(joint_membership, index, count, fuzzy_struct, 0);
|
|
}
|
|
|
|
struct PID* raw_fuzzy_pid_init(float kp,
|
|
float ki,
|
|
float kd,
|
|
float integral_limit,
|
|
float dead_zone,
|
|
float feed_forward,
|
|
float error_max,
|
|
float delta_error_max,
|
|
float delta_kp_max,
|
|
float delta_ki_max,
|
|
float delta_kd_max,
|
|
unsigned int mf_type,
|
|
unsigned int fo_type,
|
|
unsigned int df_type,
|
|
int mf_params[],
|
|
int rule_base[][qf_default],
|
|
int output_min_value,
|
|
int output_middle_value,
|
|
int output_max_value) {
|
|
struct PID* pid = (struct PID*)pika_platform_malloc(sizeof(struct PID));
|
|
pid->kp = kp;
|
|
pid->ki = ki;
|
|
pid->kd = kd;
|
|
|
|
pid->delta_kp_max = delta_kp_max;
|
|
pid->delta_ki_max = delta_ki_max;
|
|
pid->delta_kd_max = delta_kd_max;
|
|
|
|
pid->delta_kp = 0;
|
|
pid->delta_ki = 0;
|
|
pid->delta_kd = 0;
|
|
|
|
pid->error_max = error_max;
|
|
pid->delta_error_max = delta_error_max;
|
|
|
|
int output_count = 1;
|
|
if (ki > 1e-4) {
|
|
output_count += 1;
|
|
if (kd > 1e-4)
|
|
output_count += 1;
|
|
}
|
|
|
|
pid->fuzzy_struct = fuzzy_init(2, output_count);
|
|
fuzzy_params_init(pid->fuzzy_struct, mf_type, fo_type, df_type, mf_params,
|
|
rule_base);
|
|
|
|
pid->last_error = 0;
|
|
pid->current_error = 0;
|
|
|
|
pid->intergral = 0;
|
|
pid->intergral_limit = integral_limit;
|
|
|
|
pid->dead_zone = dead_zone;
|
|
pid->feed_forward = feed_forward;
|
|
|
|
pid->output_max_value = output_max_value;
|
|
pid->output_middle_value = output_middle_value;
|
|
pid->output_min_value = output_min_value;
|
|
|
|
return pid;
|
|
}
|
|
|
|
struct PID* fuzzy_pid_init(float* params,
|
|
float delta_k,
|
|
unsigned int mf_type,
|
|
unsigned int fo_type,
|
|
unsigned int df_type,
|
|
int mf_params[],
|
|
int rule_base[][qf_default]) {
|
|
return raw_fuzzy_pid_init(
|
|
params[0], params[1], params[2], params[3], params[4], params[5],
|
|
max_error, max_delta_error, params[0] / delta_k, params[1] / delta_k,
|
|
params[2] / delta_k, mf_type, fo_type, df_type, mf_params, rule_base,
|
|
min_pwm_output, middle_pwm_output, max_pwm_output);
|
|
}
|
|
|
|
struct PID* raw_pid_init(float kp,
|
|
float ki,
|
|
float kd,
|
|
float integral_limit,
|
|
float dead_zone,
|
|
float feed_forward,
|
|
float linear_adaptive_kp,
|
|
float error_max,
|
|
float delta_error_max,
|
|
int output_min_value,
|
|
int output_middle_value,
|
|
int output_max_value) {
|
|
struct PID* pid = (struct PID*)pika_platform_malloc(sizeof(struct PID));
|
|
pid->kp = kp;
|
|
pid->ki = ki;
|
|
pid->kd = kd;
|
|
|
|
pid->delta_kp_max = 0;
|
|
pid->delta_ki_max = 0;
|
|
pid->delta_kd_max = 0;
|
|
|
|
pid->delta_kp = 0;
|
|
pid->delta_ki = 0;
|
|
pid->delta_kd = 0;
|
|
|
|
pid->error_max = error_max;
|
|
pid->delta_error_max = delta_error_max;
|
|
|
|
pid->fuzzy_struct = NULL;
|
|
|
|
pid->last_error = 0;
|
|
pid->current_error = 0;
|
|
|
|
pid->intergral = 0;
|
|
pid->intergral_limit = integral_limit;
|
|
|
|
pid->dead_zone = dead_zone;
|
|
pid->feed_forward = feed_forward;
|
|
|
|
pid->output_max_value = output_max_value;
|
|
pid->output_middle_value = output_middle_value;
|
|
pid->output_min_value = output_min_value;
|
|
|
|
pid->linear_adaptive_kp = linear_adaptive_kp;
|
|
return pid;
|
|
}
|
|
|
|
struct PID* pid_init(float* params) {
|
|
return raw_pid_init(params[0], params[1], params[2], params[3], params[4],
|
|
params[5], params[6], max_error, max_delta_error,
|
|
min_pwm_output, middle_pwm_output, max_pwm_output);
|
|
}
|
|
|
|
int round_user(float parameter) {
|
|
if ((int)(parameter * 10.0) % 10 >= 5)
|
|
return parameter + 1;
|
|
else
|
|
return parameter;
|
|
}
|
|
|
|
int limit(int value, int max_limit, int min_limit) {
|
|
if (value > max_limit)
|
|
return max_limit;
|
|
if (value < min_limit)
|
|
return min_limit;
|
|
return value;
|
|
}
|
|
|
|
float fuzzy_pid_control(float real, float idea, struct PID* pid) {
|
|
pid->last_error = pid->current_error;
|
|
pid->current_error = idea - real;
|
|
float delta_error = pid->current_error - pid->last_error;
|
|
#ifdef fuzzy_pid_dead_zone
|
|
if (pid->current_error < pid->dead_zone &&
|
|
pid->current_error > -pid->dead_zone) {
|
|
pid->current_error = 0;
|
|
} else {
|
|
if (pid->current_error > pid->dead_zone)
|
|
pid->current_error = pid->current_error - pid->dead_zone;
|
|
else {
|
|
if (pid->current_error < -pid->dead_zone)
|
|
pid->current_error = pid->current_error + pid->dead_zone;
|
|
}
|
|
}
|
|
#endif
|
|
fuzzy_control(pid->current_error / pid->error_max * 3.0f,
|
|
delta_error / pid->delta_error_max * 3.0f, pid->fuzzy_struct);
|
|
|
|
pid->delta_kp =
|
|
pid->fuzzy_struct->output[0] / 3.0f * pid->delta_kp_max + pid->kp;
|
|
|
|
if (pid->fuzzy_struct->output_num >= 2)
|
|
pid->delta_ki = pid->fuzzy_struct->output[1] / 3.0f * pid->delta_ki_max;
|
|
else
|
|
pid->delta_ki = 0;
|
|
|
|
if (pid->fuzzy_struct->output_num >= 3)
|
|
pid->delta_kd = pid->fuzzy_struct->output[2] / 3.0f * pid->delta_kd_max;
|
|
else
|
|
pid->delta_ki = 0;
|
|
|
|
#ifdef fuzzy_pid_debug_print
|
|
pika_platform_printf("kp : %f, ki : %f, kd : %f\n", kp, ki, kd);
|
|
#endif
|
|
|
|
pid->intergral += (pid->ki + pid->delta_ki) * pid->current_error;
|
|
#ifdef fuzzy_pid_integral_limit
|
|
if (pid->intergral > pid->intergral_limit)
|
|
pid->intergral = pid->intergral_limit;
|
|
else {
|
|
if (pid->intergral < -pid->intergral_limit)
|
|
pid->intergral = -pid->intergral_limit;
|
|
}
|
|
#endif
|
|
pid->output =
|
|
(pid->kp + pid->delta_kp) * pid->current_error + pid->intergral +
|
|
(pid->kd + pid->delta_kd) * (pid->current_error - pid->last_error);
|
|
pid->output += pid->feed_forward * (float)idea;
|
|
return pid->output;
|
|
}
|
|
|
|
float pid_control(float real, float idea, struct PID* pid) {
|
|
pid->last_error = pid->current_error;
|
|
pid->current_error = idea - real;
|
|
|
|
#ifdef pid_dead_zone
|
|
if (pid->current_error < pid->dead_zone &&
|
|
pid->current_error > -pid->dead_zone) {
|
|
pid->current_error = 0;
|
|
} else {
|
|
if (pid->current_error > pid->dead_zone)
|
|
pid->current_error = pid->current_error - pid->dead_zone;
|
|
else {
|
|
if (pid->current_error < -pid->dead_zone)
|
|
pid->current_error = pid->current_error + pid->dead_zone;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
#ifdef pid_debug_print
|
|
pika_platform_printf("kp : %f, ki : %f, kd : %f\n", kp, ki, kd);
|
|
#endif
|
|
|
|
pid->intergral += (pid->ki) * pid->current_error;
|
|
#ifdef pid_integral_limit
|
|
if (pid->intergral > pid->intergral_limit)
|
|
pid->intergral = pid->intergral_limit;
|
|
else {
|
|
if (pid->intergral < -pid->intergral_limit)
|
|
pid->intergral = -pid->intergral_limit;
|
|
}
|
|
#endif
|
|
|
|
float linear_adaptive_kp = 1;
|
|
if (pid->linear_adaptive_kp > 1e-4)
|
|
linear_adaptive_kp = (1 - pid->linear_adaptive_kp) *
|
|
pid->current_error / pid->error_max +
|
|
pid->linear_adaptive_kp;
|
|
|
|
pid->output = pid->kp * linear_adaptive_kp * pid->current_error +
|
|
pid->intergral +
|
|
(pid->kd) * (pid->current_error - pid->last_error);
|
|
pid->output += pid->feed_forward * (float)idea;
|
|
return pid->output;
|
|
}
|
|
|
|
void delete_pid(struct PID* pid) {
|
|
if (pid->fuzzy_struct != NULL) {
|
|
delete_fuzzy(pid->fuzzy_struct);
|
|
}
|
|
pika_platform_free(pid);
|
|
}
|
|
|
|
void delete_pid_vector(struct PID** pid_vector, unsigned int count) {
|
|
for (unsigned int i = 0; i < count; ++i) {
|
|
delete_pid(pid_vector[i]);
|
|
}
|
|
pika_platform_free(pid_vector);
|
|
}
|
|
|
|
struct PID** pid_vector_init(float params[][pid_params_count],
|
|
unsigned int count) {
|
|
struct PID** pid =
|
|
(struct PID**)pika_platform_malloc(sizeof(struct PID*) * count);
|
|
for (unsigned int i = 0; i < count; ++i) {
|
|
pid[i] = pid_init(params[i]);
|
|
}
|
|
return pid;
|
|
}
|
|
|
|
struct PID** fuzzy_pid_vector_init(float params[][pid_params_count],
|
|
float delta_k,
|
|
unsigned int mf_type,
|
|
unsigned int fo_type,
|
|
unsigned int df_type,
|
|
int* mf_params,
|
|
int rule_base[][qf_default],
|
|
unsigned int count) {
|
|
struct PID** pid =
|
|
(struct PID**)pika_platform_malloc(sizeof(struct PID*) * count);
|
|
for (unsigned int i = 0; i < count; ++i) {
|
|
pid[i] = fuzzy_pid_init(params[i], delta_k, mf_type, fo_type, df_type,
|
|
mf_params, rule_base);
|
|
}
|
|
return pid;
|
|
}
|
|
|
|
int direct_control(int zero_value, int offset_value, pika_bool direct) {
|
|
if (direct == pika_true) {
|
|
return zero_value + offset_value;
|
|
} else {
|
|
return zero_value - offset_value;
|
|
}
|
|
}
|
|
|
|
int fuzzy_pid_motor_pwd_output(float real,
|
|
float idea,
|
|
pika_bool direct,
|
|
struct PID* pid) {
|
|
return limit(direct_control(pid->output_middle_value,
|
|
fuzzy_pid_control(real, idea, pid), direct),
|
|
pid->output_max_value, pid->output_min_value);
|
|
}
|
|
|
|
int pid_motor_pwd_output(float real,
|
|
float idea,
|
|
pika_bool direct,
|
|
struct PID* pid) {
|
|
return limit(direct_control(pid->output_middle_value,
|
|
pid_control(real, idea, pid), direct),
|
|
pid->output_max_value, pid->output_min_value);
|
|
}
|