1
0
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:
Rene Hopf 2014-09-16 15:51:44 +02:00
parent 4e6365db40
commit f83024b4fc
6 changed files with 266 additions and 41 deletions

View File

@ -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

View File

@ -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
View 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
View 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

View File

@ -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

View File

@ -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;