From 4e3f537f3e0bcb0db6dc843437ab6bbaa31fa992 Mon Sep 17 00:00:00 2001 From: Rene Hopf Date: Tue, 25 Nov 2014 01:40:44 +0100 Subject: [PATCH] pid -> misc --- Makefile | 2 +- src/misc.c | 43 ++++++ src/misc.h | 42 ++++++ src/pid.c | 399 ---------------------------------------------------- src/pid.h | 192 ------------------------- src/setup.c | 3 - src/setup.h | 5 +- 7 files changed, 87 insertions(+), 599 deletions(-) create mode 100644 src/misc.c create mode 100644 src/misc.h delete mode 100644 src/pid.c delete mode 100644 src/pid.h diff --git a/Makefile b/Makefile index d7fef290..1c92cf3d 100644 --- a/Makefile +++ b/Makefile @@ -1,6 +1,6 @@ # put your *.o targets here, make should handle the rest! -SRCS = main.c stm32f4xx_it.c system_stm32f4xx.c printf.c scanf.c param.c setup.c stm32_ub_encoder_tim3.c pid.c hal.c +SRCS = main.c stm32f4xx_it.c system_stm32f4xx.c printf.c scanf.c param.c setup.c hal.c misc.c #USB SRCS += ub_lib/stm32_ub_usb_cdc.c ub_lib/usb_cdc_lolevel/usb_core.c ub_lib/usb_cdc_lolevel/usb_dcd_int.c ub_lib/usb_cdc_lolevel/usbd_req.c ub_lib/usb_cdc_lolevel/usbd_cdc_core.c ub_lib/usb_cdc_lolevel/usbd_core.c ub_lib/usb_cdc_lolevel/usb_dcd.c ub_lib/usb_cdc_lolevel/usbd_cdc_vcp.c ub_lib/usb_cdc_lolevel/usbd_desc.c ub_lib/usb_cdc_lolevel/usbd_ioreq.c ub_lib/usb_cdc_lolevel/usb_bsp.c ub_lib/usb_cdc_lolevel/usbd_usr.c #SRCS = main.c system.c diff --git a/src/misc.c b/src/misc.c new file mode 100644 index 00000000..7cd6c448 --- /dev/null +++ b/src/misc.c @@ -0,0 +1,43 @@ +/* + * This file is part of the stmbl project. + * + * Copyright (C) 2013-2015 Rene Hopf + * Copyright (C) 2013-2015 Nico Stute + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "misc.h" + +float minus(float a, float b){ + if(ABS(a - b) < M_PI){ + return(a - b); + } + else if(a > b){ + return(a - b - 2.0 * M_PI); + } + else{ + return(a - b + 2.0 * M_PI); + } +} + +float mod(float a){ + while(a < -M_PI){ + a += 2.0 * M_PI; + } + while(a > M_PI){ + a -= 2.0 * M_PI; + } + return(a); +} diff --git a/src/misc.h b/src/misc.h new file mode 100644 index 00000000..02ec0241 --- /dev/null +++ b/src/misc.h @@ -0,0 +1,42 @@ +/* + * This file is part of the stmbl project. + * + * Copyright (C) 2013-2015 Rene Hopf + * Copyright (C) 2013-2015 Nico Stute + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define ABS(a) (((a) < 0) ? -(a) : (a)) +#define CLAMP(x, low, high) (((x) > (high)) ? (high) : (((x) < (low)) ? (low) : (x))) +#define LIMIT(x, lowhigh) (((lowhigh) > 0.0) ? (((x) > (lowhigh)) ? (lowhigh) : (((x) < (-lowhigh)) ? (-lowhigh) : (x))) : (x)) +#define MIN(a, b) (((a) < (b)) ? (a) : (b)) +#define MAX(a, b) (((a) > (b)) ? (a) : (b)) +#define DEG(a) ((a)*M_PI/180.0) +#define RAD(a) ((a)*180.0/M_PI) +#define SIGN(a) (((a) < 0) ? (-1) : (((a) > 0) ? (1) : (0))) + +float minus(float a, float b); +float mod(float a); + +#ifdef __cplusplus +} +#endif diff --git a/src/pid.c b/src/pid.c deleted file mode 100644 index 9ecfdd27..00000000 --- a/src/pid.c +++ /dev/null @@ -1,399 +0,0 @@ -/* - * This file is part of the stmbl project. - * - * Copyright (C) 2013-2015 Rene Hopf - * Copyright (C) 2013-2015 Nico Stute - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -#include "pid.h" - -float minus(float a, float b){ - if(ABS(a - b) < M_PI){ - return(a - b); - } - else if(a > b){ - return(a - b - 2.0 * M_PI); - } - else{ - return(a - b + 2.0 * M_PI); - } -} - -float mod(float a){ - while(a < -M_PI){ - a += 2.0 * M_PI; - } - while(a > M_PI){ - a -= 2.0 * M_PI; - } - return(a); -} - -void pid_init(hal_pid_t *pid){ - pid->enable = 0; /* pin: enable input */ - //pid->command = 0; /* pin: commanded value */ - //pid->commandvds = 0; /* pin: commanded derivative dummysig */ - pid->commandv = 0; /* pin: commanded derivative value */ - pid->commanda = 0; /* pin: commandedv derivative value */ - //pid->feedback = 0; /* pin: feedback value */ - //pid->feedbackvds = 0; /* pin: feedback derivative dummysig */ - pid->feedbackv = 0; /* pin: feedback derivative value */ - pid->feedbacka = 0; /* pin: feedbackv derivative value */ - pid->error = 0; /* pin: command - feedback */ - pid->deadband = 0; /* pin: deadband */ - pid->maxerror = 0; /* pin: limit for error */ - pid->maxerror_i = 0; /* pin: limit for integrated error */ - pid->maxerror_d = 0; /* pin: limit for differentiated error */ - pid->maxcmd_d = 0; /* pin: limit for differentiated cmd */ - pid->maxcmd_dd = 0; /* pin: limit for 2nd derivative of cmd */ - pid->error_i = 0; /* opt. pin: integrated error */ - pid->prev_error = 0; /* previous error for differentiator */ - pid->error_d = 0; /* opt. pin: differentiated error */ - pid->error_dd = 0; /* opt. pin: differentiated error */ - //pid->prev_cmd = 0; /* previous command for differentiator */ - //pid->prev_fb = 0; /* previous feedback for differentiator */ - pid->limit_state = 0; /* +1 or -1 if in limit, else 0.0 */ - pid->cmd_d = 0; /* opt. pin: differentiated command */ - pid->cmd_dd = 0; /* opt. pin: 2nd derivative of command */ - pid->bias = 0; /* param: steady state offset */ - pid->pgain = 2; /* pin: proportional gain */ - pid->igain = 0.04; /* pin: integral gain */ - pid->dgain = 0.015; /* pin: derivative gain */ - pid->bgain = 0.0; /* pin: derivative gain */ - // pid->ff0gain = 0; /* pin: feedforward proportional */ - pid->ff1gain = 0.005; /* pin: feedforward derivative */ - pid->ff2gain = 0.00005; /* pin: feedforward 2nd derivative */ - pid->maxoutput = 1; /* pin: limit for PID output */ - pid->output = 0; /* pin: the output value */ - pid->saturated = 0; /* pin: TRUE when the output is saturated */ - pid->saturated_s = 0; /* pin: the time the output has been saturated */ - pid->saturated_count = 0; /* pin: the time the output has been saturated */ - //pid->index_enable = 0;/* pin: to monitor for step changes that would otherwise screw up FF */ - //pid->error_previous_target = 0; /* pin: measure error as new position vs previous command, to match motion's ideas */ - //pid->prev_ie = 0; -} - -void calc_pid(hal_pid_t *arg, float period) -{ - hal_pid_t *pid; - float tmp1; - int enable; - - /* point to the data for this PID loop */ - pid = arg; - /* precalculate some timing constants */ - /* get the enable bit */ - enable = pid->enable; - /* read the command and feedback only once */ - //command = pid->command; - //feedback = pid->feedback; - - - // /* calculate the error */ - // if((!(pid->prev_ie && !pid->index_enable)) && - // (pid->error_previous_target)) { - // // the user requests ferror against prev_cmd, and we can honor - // // that request because we haven't just had an index reset that - // // screwed it up. Otherwise, if we did just have an index - // // reset, we will present an unwanted ferror proportional to - // // velocity for this period, but velocity is usually very small - // // during index search. - // tmp1 = pid->prev_cmd - feedback; - // } else { - // tmp1 = command - feedback; - // } - // /* store error to error pin */ - // pid->error = tmp1; - tmp1 = pid->error; - - /* apply error limits */ - if (pid->maxerror != 0.0) { - if (tmp1 > pid->maxerror) { - tmp1 = pid->maxerror; - } else if (tmp1 < -pid->maxerror) { - tmp1 = -pid->maxerror; - } - } - /* apply the deadband */ - if (tmp1 > pid->deadband) { - tmp1 -= pid->deadband; - } else if (tmp1 < -pid->deadband) { - tmp1 += pid->deadband; - } else { - tmp1 = 0; - } - /* do integrator calcs only if enabled */ - if (enable != 0 && pid->igain != 0) { - /* if output is in limit, don't let integrator wind up */ - if ( ( tmp1 * pid->limit_state ) <= 0.0 ) { - /* compute integral term */ - pid->error_i += tmp1 * period; - } - /* apply integrator limits */ - if (pid->maxerror_i != 0.0) { - if (pid->error_i > pid->maxerror_i) { - pid->error_i = pid->maxerror_i; - } else if (pid->error_i < -pid->maxerror_i) { - pid->error_i = -pid->maxerror_i; - } - } - } else { - /* not enabled, reset integrator */ - pid->error_i = 0; - } - /* compute command and feedback derivatives to dummysigs */ - // if(!(pid->prev_ie && !pid->index_enable)) { - // pid->commandvds = (command - pid->prev_cmd) * periodrecip; - // pid->feedbackvds = (feedback - pid->prev_fb) * periodrecip; - // } - /* and calculate derivative term as difference of derivatives */ - //tmp2 = pid->error_d; - pid->error_d = pid->commandv - pid->feedbackv; - //pid->prev_error = tmp1; - /* apply derivative limits */ - if (pid->maxerror_d != 0.0) { - if (pid->error_d > pid->maxerror_d) { - pid->error_d = pid->maxerror_d; - } else if (pid->error_d < -pid->maxerror_d) { - pid->error_d = -pid->maxerror_d; - } - } - - //pid->error_dd = (pid->error_d - tmp2) * periodrecip; - pid->error_dd = pid->commanda - pid->feedbacka; - /* apply 2nd derivative limits */ - if (pid->maxerror_dd != 0.0) { - if (pid->error_dd > pid->maxerror_dd) { - pid->error_dd = pid->maxerror_dd; - } else if (pid->error_dd < -pid->maxerror_dd) { - pid->error_dd = -pid->maxerror_dd; - } - } - /* calculate derivative of command */ - /* save old value for 2nd derivative calc later */ - //tmp2 = pid->cmd_d; - //if(!(pid->prev_ie && !pid->index_enable)) { - // not falling edge of index_enable: the normal case - pid->cmd_d = pid->commandv; - //} - // else: leave cmd_d alone and use last period's. prev_cmd - // shouldn't be trusted because index homing has caused us to have - // a step in position. Using the previous period's derivative is - // probably a decent approximation since index search is usually a - // slow steady speed. - - // save ie for next time - // pid->prev_ie = pid->index_enable; - - // pid->prev_cmd = command; - // pid->prev_fb = feedback; - - /* apply derivative limits */ - if (pid->maxcmd_d != 0.0) { - if (pid->cmd_d > pid->maxcmd_d) { - pid->cmd_d = pid->maxcmd_d; - } else if (pid->cmd_d < -pid->maxcmd_d) { - pid->cmd_d = -pid->maxcmd_d; - } - } - /* calculate 2nd derivative of command */ - //pid->cmd_dd = (pid->cmd_d - tmp2) * periodrecip * 0.5 + pid->cmd_dd * 0.5; - pid->cmd_dd = pid->commanda; - /* apply 2nd derivative limits */ - if (pid->maxcmd_dd != 0.0) { - if (pid->cmd_dd > pid->maxcmd_dd) { - pid->cmd_dd = pid->maxcmd_dd; - } else if (pid->cmd_dd < -pid->maxcmd_dd) { - pid->cmd_dd = -pid->maxcmd_dd; - } - } - /* do output calcs only if enabled */ - if (enable != 0) { - /* calculate the output value */ - tmp1 = - pid->bias + pid->pgain * tmp1 + pid->igain * pid->error_i + - pid->dgain * pid->error_d + pid->bgain * pid->error_dd; - tmp1 += /*command * pid->ff0gain + */ pid->cmd_d * pid->ff1gain + - pid->cmd_dd * pid->ff2gain; - /* apply output limits */ - if (pid->maxoutput != 0.0) { - if (tmp1 > pid->maxoutput) { - tmp1 = pid->maxoutput; - pid->limit_state = 1.0; - } else if (tmp1 < -pid->maxoutput) { - tmp1 = -pid->maxoutput; - pid->limit_state = -1.0; - } else { - pid->limit_state = 0.0; - } - } - } else { - /* not enabled, force output to zero */ - tmp1 = 0.0; - pid->limit_state = 0.0; - } - /* write final output value to output pin */ - pid->output = tmp1; - - /* set 'saturated' outputs */ - if(pid->limit_state) { - pid->saturated = 1; - pid->saturated_s += period; - if(pid->saturated_count != 2147483647) - (pid->saturated_count) ++; - } else { - pid->saturated = 0; - pid->saturated = 0; - pid->saturated_count = 0; - } - /* done */ -} - - - -void pid2(pid2p* pid){ - if(pid->enable > 0.5){ - // pos -> vel - pid->pos_error = minus(pid->ext_cmd_pos, pid->feedback_pos); - pid->cmd_vel = LIMIT(pid->pos_p * pid->pos_error + pid->ff1 * pid->ext_cmd_vel, pid->max_vel) * pid->pos_lpass + pid->cmd_vel * (1 - pid->pos_lpass); - - // vel -> acc - pid->vel_error = pid->cmd_vel - pid->feedback_vel; - if(pid->vel_i > 0.0){ - pid->vel_error_sum = LIMIT(pid->vel_error_sum + pid->vel_error * pid->period, pid->max_vel_error_sum); - } - else{ - pid->vel_error_sum = 0.0; - } - pid->cmd_acc = LIMIT(pid->vel_p * pid->vel_error + pid->vel_i * pid->vel_error_sum + pid->ff2 * pid->ext_cmd_acc, pid->max_acc) * pid->vel_lpass + pid->cmd_acc * (1 - pid->vel_lpass); - - // acc -> force - pid->cmd_force = LIMIT(pid->acc_p * pid->cmd_acc + pid->ext_cmd_force, pid->max_force); - - // force -> current - pid->cmd_cur = LIMIT(pid->force_p * pid->cmd_force, pid->max_cur); - - // current -> volt - pid->cur_error = pid->cmd_cur - pid->feedback_cur; - pid->cmd_volt = LIMIT(pid->cur_p * pid->cur_error + pid->cur_d * (pid->cmd_cur - pid->cmd_cur_old) / pid->period + pid->ind_p * pid->feedback_vel, pid->max_volt) * pid->cur_lpass + pid->cmd_volt * (1 - pid->cur_lpass); - pid->cmd_cur_old = pid->cmd_cur; - - // volt -> pwm - pid->cmd_pwm = pid->cmd_volt / pid->volt; - if(pid->cmd_pwm >= pid->max_pwm || pid->cmd_pwm <= -pid->max_pwm){ - pid->saturated_s += pid->period; - } - else{ - pid->saturated_s = 0.0; - } - pid->cmd_pwm = LIMIT(pid->cmd_pwm, pid->max_pwm); - } - else{ - pid->cmd_vel = 0.0; - pid->cmd_acc = 0.0; - pid->cmd_force = 0.0; - pid->cmd_cur = 0.0; - pid->cmd_volt = 0.0; - pid->cmd_vel = 0.0; - - pid->vel_error_sum = 0.0; - - pid->cmd_cur_old = 0.0; - - pid->saturated_s = 0.0; - } -} - - -void pid2_init(pid2p* pid){ - pid->ext_cmd_pos = 0.0; - pid->feedback_pos = 0.0; - - pid->cmd_vel = 0.0; - pid->ext_cmd_vel = 0.0; - pid->feedback_vel = 0.0; - - pid->cmd_acc = 0.0; - pid->ext_cmd_acc = 0.0; - //pid->feedback_acc = 0.0; - - pid->cmd_force = 0.0; - pid->ext_cmd_force = 0.0; - //pid->feedback_force = 0.0; - - pid->cmd_cur = 0.0; - pid->feedback_cur = 0.0; - - pid->cmd_volt = 0.0; - - pid->cmd_pwm = 0.0; - - - pid->pos_error = 0.0; - pid->vel_error = 0.0; - pid->cur_error = 0.0; - - pid->enable = 1.0; - - pid->pos_p = 30.0; - pid->ff1 = 0.95; - pid->pos_lpass = 1.0; - - pid->vel_p = 1.0; - pid->vel_i = 40.0; - pid->ff2 = 0.002; - pid->vel_lpass = 1.0; - - pid->acc_p = 0.1; - - pid->force_p = 3.667; - - pid->cur_p = 15.0; - pid->cur_d = 0.01; - pid->ind_p = 0.57; - pid->cur_lpass = 1.0; - - pid->volt = 130.0; - - pid->period = 0.001; - - - pid->max_vel = 62.9; - pid->max_vel_error_sum = 2.5; - - pid->max_acc = 1200; - - pid->max_force = 100.0; - - pid->max_cur = 6.0; - - pid->max_volt = 130.0; - - pid->max_pwm = 0.9; - - pid->i0 = 1.0; - - - pid->vel_error_sum = 0.0; - - pid->cmd_cur_old = 0.0; - - pid->saturated_s = 0.0; - - pid->i2t = 0.0; - - pid->minus = minus; -} diff --git a/src/pid.h b/src/pid.h deleted file mode 100644 index eeeec153..00000000 --- a/src/pid.h +++ /dev/null @@ -1,192 +0,0 @@ -/* - * This file is part of the stmbl project. - * - * Copyright (C) 2013-2015 Rene Hopf - * Copyright (C) 2013-2015 Nico Stute - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -#pragma once -#include - -#ifdef __cplusplus -extern "C" { -#endif - -#define ABS(a) (((a) < 0) ? -(a) : (a)) -#define CLAMP(x, low, high) (((x) > (high)) ? (high) : (((x) < (low)) ? (low) : (x))) -#define LIMIT(x, lowhigh) (((lowhigh) > 0.0) ? (((x) > (lowhigh)) ? (lowhigh) : (((x) < (-lowhigh)) ? (-lowhigh) : (x))) : (x)) -#define MIN(a, b) (((a) < (b)) ? (a) : (b)) -#define MAX(a, b) (((a) > (b)) ? (a) : (b)) -#define DEG(a) ((a)*M_PI/180.0) -#define RAD(a) ((a)*180.0/M_PI) -#define SIGN(a) (((a) < 0) ? (-1) : (((a) > 0) ? (1) : (0))) - -typedef struct { - int enable; /* pin: enable input */ - //float command; /* pin: commanded value */ - //float commandvds; /* pin: commanded derivative dummysig */ - float commandv; /* pin: commanded derivative value */ - float commanda; /* pin: commandedv derivative value */ - //float feedback; /* pin: feedback value */ - //float feedbackvds; /* pin: feedback derivative dummysig */ - float feedbackv; /* pin: feedback derivative value */ - float feedbacka; /* pin: feedbackv derivative value */ - float error; /* pin: command - feedback */ - float deadband; /* pin: deadband */ - float maxerror; /* pin: limit for error */ - float maxerror_i; /* pin: limit for integrated error */ - float maxerror_d; /* pin: limit for differentiated error */ - float maxerror_dd; /* pin: limit for differentiated error */ - float maxcmd_d; /* pin: limit for differentiated cmd */ - float maxcmd_dd; /* pin: limit for 2nd derivative of cmd */ - float error_i; /* opt. pin: integrated error */ - float prev_error; /* previous error for differentiator */ - float error_d; /* opt. pin: differentiated error */ - float error_dd; /* opt. pin: differentiated error */ - //float prev_cmd; /* previous command for differentiator */ - //float prev_fb; /* previous feedback for differentiator */ - float limit_state; /* +1 or -1 if in limit, else 0.0 */ - float cmd_d; /* opt. pin: differentiated command */ - float cmd_dd; /* opt. pin: 2nd derivative of command */ - float bias; /* param: steady state offset */ - float pgain; /* pin: proportional gain */ - float igain; /* pin: integral gain */ - float dgain; /* pin: derivative gain */ - float bgain; /* pin: derivative gain */ - // float ff0gain; /* pin: feedforward proportional */ - float ff1gain; /* pin: feedforward derivative */ - float ff2gain; /* pin: feedforward 2nd derivative */ - float maxoutput; /* pin: limit for PID output */ - float output; /* pin: the output value */ - int saturated; /* pin: TRUE when the output is saturated */ - float saturated_s; /* pin: the time the output has been saturated */ - int saturated_count; /* pin: the time the output has been saturated */ - //int index_enable; /* pin: to monitor for step changes that would otherwise screw up FF */ - //int error_previous_target; /* pin: measure error as new position vs previous command, to match motion's ideas */ - //char prev_ie; -} hal_pid_t; - -typedef struct { - // pos - float ext_cmd_pos; - float feedback_pos; - - // vel - float cmd_vel; - float ext_cmd_vel; - float feedback_vel; - - // acc - float cmd_acc; - float ext_cmd_acc; - //float feedback_acc; - - // force - float cmd_force; - float ext_cmd_force; - //float feedback_force; - - // current - float cmd_cur; - float feedback_cur; - - // volt - float cmd_volt; - - // pwm output - float cmd_pwm; - - - // error - float pos_error; - float vel_error; - float cur_error; - - - // parameters - float enable; - - // pos -> vel - float pos_p; - float ff1; - float pos_lpass; - - // vel -> acc - float vel_p; - float vel_i; - float ff2; - float vel_lpass; - - // acc -> force - float acc_p; - - // force -> current - float force_p; - - // current -> volt - float cur_p; - float cur_d; - float ind_p; - float cur_lpass; - - // voltage -> pwm - float volt; - - // time - float period; - - // limits - float max_vel; - float max_vel_error_sum; - - float max_acc; - - float max_force; - - float max_cur; - - float max_volt; - - float max_pwm; - - float i0; - - - // state - float vel_error_sum; - - float cmd_cur_old; - - float saturated_s; - - float i2t; - - // operator - // pos_error = minus(cmd_pos, feedback_pos) - float (*minus)(float a, float b); -}pid2p; - -void pid2(pid2p* pid); -void pid2_init(pid2p* pid); - -void calc_pid(hal_pid_t *arg, float period); -void pid_init(hal_pid_t *pid); -float minus(float a, float b); -float mod(float a); - -#ifdef __cplusplus -} -#endif diff --git a/src/setup.c b/src/setup.c index be684619..8ebef756 100644 --- a/src/setup.c +++ b/src/setup.c @@ -64,9 +64,6 @@ void setup(){ NVIC_SetPriority(SysTick_IRQn, 14); - pid_init(&pid); - pid2_init(&pid2ps); - #ifdef USBTERM UB_USB_CDC_Init(); #endif diff --git a/src/setup.h b/src/setup.h index 1d0f47d4..d6f2473d 100644 --- a/src/setup.h +++ b/src/setup.h @@ -11,8 +11,7 @@ #include #include -#include "stm32_ub_encoder_tim3.h" -#include "pid.h" +#include "misc.h" #ifdef USBTERM #include "stm32_ub_usb_cdc.h" @@ -53,8 +52,6 @@ void setup_dma(); void SysTick_Handler(void); volatile int time; -hal_pid_t pid; -pid2p pid2ps; volatile uint32_t ADC2_DMA_Buffer[ADC2d_ANZ];