mirror of
https://github.com/rene-dev/stmbl.git
synced 2024-12-26 02:22:25 +00:00
neuer pid
This commit is contained in:
parent
4e6365db40
commit
f83024b4fc
2
Makefile
2
Makefile
@ -1,6 +1,6 @@
|
||||
# put your *.o targets here, make should handle the rest!
|
||||
|
||||
SRCS = main.c stm32f4xx_it.c system_stm32f4xx.c stlinky.c printf.c scanf.c param.c stm32_ub_dac_dma.c setup.c stm32_ub_encoder_tim3.c
|
||||
SRCS = main.c stm32f4xx_it.c system_stm32f4xx.c stlinky.c printf.c scanf.c param.c stm32_ub_dac_dma.c setup.c stm32_ub_encoder_tim3.c pid.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
|
||||
|
51
src/main.c
51
src/main.c
@ -48,7 +48,6 @@ volatile float res_pos2;//winkel vom resolver, -pi bsi +pi
|
||||
volatile int amp1,amp2;//betrag
|
||||
volatile int erreger = 0;//resolver erreger pin an/aus
|
||||
volatile int erreger_enable = NO;//erreger aktiv
|
||||
volatile float schleppfehler = 0;
|
||||
|
||||
float minus(float a, float b){
|
||||
if(ABS(a - b) < pi){
|
||||
@ -142,44 +141,17 @@ void ADC_IRQHandler(void) // 20khz
|
||||
erreger = !erreger; // 10khz
|
||||
}
|
||||
|
||||
struct pid_context{
|
||||
float old_ist;
|
||||
float i_sum;
|
||||
float p;
|
||||
float i;
|
||||
float d;
|
||||
float periode;
|
||||
float i_max;
|
||||
float v;
|
||||
} c;
|
||||
|
||||
float pid(float ist, float soll, struct pid_context* context){
|
||||
float ctr = 0;
|
||||
float err = minus(soll, ist);
|
||||
schleppfehler = err;//TODO: in kontext oder so
|
||||
float p = context->p / context->v * 10;
|
||||
float i = context->i / context->v * 10;
|
||||
float d = context->d / context->v * 10;
|
||||
|
||||
context->i_sum = CLAMP(context->i_sum + err * context->periode, -context->i_max / i, context->i_max / i);
|
||||
|
||||
ctr = p * err + i * context->i_sum;
|
||||
return(CLAMP(ctr, -1, 1));
|
||||
}
|
||||
|
||||
void TIM5_IRQHandler(void){ //1KHz
|
||||
TIM_ClearITPendingBit(TIM5, TIM_IT_Update);
|
||||
c.v = 20;
|
||||
c.p = 5;
|
||||
c.periode = 0.001;
|
||||
c.i_max = 1;
|
||||
c.i = 30;
|
||||
c.d = 0;
|
||||
float ist = get_enc_pos();
|
||||
|
||||
voltage_scale = pid(ist, soll_pos, &c);
|
||||
|
||||
//if(amp1 < 1000000 || amp2 < 1000000){
|
||||
pid.feedback = minus(ist,soll_pos);
|
||||
pid.commandv = pid.commandvds;
|
||||
pid.feedbackv = pid.feedbackvds;
|
||||
calc_pid(&pid,1);
|
||||
voltage_scale = pid.output;
|
||||
|
||||
//if(amp1 < 1000000 || amp2 < 1000000){
|
||||
// voltage_scale = 0.0;
|
||||
//}
|
||||
|
||||
@ -206,20 +178,19 @@ int main(void)
|
||||
//printf_("%f %f diff: %f\r",RAD(res_pos1),RAD(res_pos2),RAD(res_pos1-res_pos2));
|
||||
//printf_("%i %i",t1_mid,t2_mid);
|
||||
//printf_("%i %i diff: %i\r",amp1,amp2,amp1-amp2);
|
||||
int e = (int)((RAD(schleppfehler)+180)/360*128)+128;
|
||||
int e = (int)((RAD(pid.error)+180)/360*128)+128;
|
||||
char buf[2];
|
||||
buf[0] = e;
|
||||
buf[1] = 0;
|
||||
//printf_("%i\r",e);
|
||||
printf_("s: %f \n", schleppfehler);
|
||||
|
||||
|
||||
printf_("e: %f\n", pid.error);
|
||||
/*
|
||||
#ifdef USBTERM
|
||||
if(UB_USB_CDC_GetStatus()==USB_CDC_CONNECTED){
|
||||
UB_USB_CDC_SendString(buf, NONE);
|
||||
}
|
||||
#endif
|
||||
|
||||
*/
|
||||
Wait(50);
|
||||
}
|
||||
}
|
||||
|
203
src/pid.c
Normal file
203
src/pid.c
Normal file
@ -0,0 +1,203 @@
|
||||
#include "pid.h"
|
||||
|
||||
void pid_init(hal_pid_t *pid){
|
||||
pid->enable = 1; /* pin: enable input */
|
||||
pid->command = 0; /* pin: commanded value */
|
||||
pid->commandvds = 0; /* pin: commanded derivative dummysig */
|
||||
pid->commandv = 0; /* pin: commanded derivative value */
|
||||
pid->feedback = 0; /* pin: feedback value */
|
||||
pid->feedbackvds = 0; /* pin: feedback derivative dummysig */
|
||||
pid->feedbackv = 0; /* pin: feedback 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->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 = 3; /* pin: proportional gain */
|
||||
pid->igain = 40; /* pin: integral gain */
|
||||
pid->dgain = 0.01; /* pin: derivative gain */
|
||||
pid->ff0gain = 0; /* pin: feedforward proportional */
|
||||
pid->ff1gain = 0; /* pin: feedforward derivative */
|
||||
pid->ff2gain = 0; /* 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, tmp2, command, feedback;
|
||||
int enable;
|
||||
float periodfp, periodrecip;
|
||||
|
||||
/* point to the data for this PID loop */
|
||||
pid = arg;
|
||||
/* precalculate some timing constants */
|
||||
periodfp = period * 0.001;
|
||||
periodrecip = 1.0 / periodfp;
|
||||
/* 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;
|
||||
/* 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) {
|
||||
/* 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 * periodfp;
|
||||
}
|
||||
/* 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 */
|
||||
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;
|
||||
}
|
||||
}
|
||||
/* 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 = (command - pid->prev_cmd) * periodrecip;
|
||||
}
|
||||
// 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;
|
||||
/* 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;
|
||||
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 * 1e-3;
|
||||
if(pid->saturated_count != 2147483647)
|
||||
(pid->saturated_count) ++;
|
||||
} else {
|
||||
pid->saturated = 0;
|
||||
pid->saturated_s = 0;
|
||||
pid->saturated_count = 0;
|
||||
}
|
||||
/* done */
|
||||
}
|
47
src/pid.h
Normal file
47
src/pid.h
Normal file
@ -0,0 +1,47 @@
|
||||
#ifndef test_pid_h
|
||||
#define test_pid_h
|
||||
|
||||
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 feedback; /* pin: feedback value */
|
||||
float feedbackvds; /* pin: feedback derivative dummysig */
|
||||
float feedbackv; /* pin: feedback 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 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 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 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;
|
||||
|
||||
void calc_pid(hal_pid_t *arg, float period);
|
||||
void pid_init(hal_pid_t *pid);
|
||||
|
||||
#endif
|
@ -79,6 +79,8 @@ void setup(){
|
||||
//systick prio
|
||||
//NVIC_SetPriority (SysTick_IRQn, (1<<__NVIC_PRIO_BITS) - 1);
|
||||
|
||||
pid_init(&pid);
|
||||
|
||||
#ifdef USBTERM
|
||||
UB_USB_CDC_Init();
|
||||
#endif
|
||||
|
@ -13,6 +13,7 @@
|
||||
#include <stm32f4xx_conf.h>
|
||||
#include "stm32_ub_dac_dma.h"
|
||||
#include "stm32_ub_encoder_tim3.h"
|
||||
#include "pid.h"
|
||||
|
||||
#ifdef USBTERM
|
||||
#include "stm32_ub_usb_cdc.h"
|
||||
@ -45,6 +46,7 @@ void setup_encoder();
|
||||
void SysTick_Handler(void);
|
||||
|
||||
volatile int time;
|
||||
hal_pid_t pid;
|
||||
|
||||
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
|
||||
TIM_OCInitTypeDef TIM_OCInitStructure;
|
||||
|
Loading…
Reference in New Issue
Block a user