1
0
mirror of https://github.com/rene-dev/stmbl.git synced 2024-12-25 01:52:13 +00:00

neuer regler

This commit is contained in:
crinq 2013-12-15 22:43:03 +01:00
parent 7ec029e3b6
commit 86f9d8074d

View File

@ -1,6 +1,3 @@
/* Michael Pratt <michael@pratt.im>
* Vastly simplified the code from https://github.com/jeremyherbert/stm32-templates/ */
#include <stm32f4_discovery.h>
#include <stm32f4xx_conf.h>
#include "../../sin.h"
@ -8,7 +5,6 @@
#include "scanf.h"
#include "stlinky.h"
#include "param.h"
//#include "stm32_ub_dac_dma.h"
#include "setup.h"
#include <math.h>
@ -23,12 +19,9 @@ void Delay(volatile uint32_t nCount);
#define RAD(a) ((a)*180.0/pi)
#define pole_count 4
#define maxdiff 2*pi/pole_count/4
//#define res_offset 0.675 (2.48+2.57)/2 = 2.525
//#define res_offset 0.675 (2.60+2.71)/2 = 2.655
//#define res_offset 0.7
float max_mag_diff;
volatile float res_offset;
#define pwm_scale 0.8
#define sin_res 1024
@ -43,18 +36,6 @@ volatile float res_offset;
#define read_neg (22+8)
#define read_w 4
volatile float mag_pos;
volatile float mag_pos_offset;
volatile float mot_pos;
volatile float res_pos;
volatile float res_pos_pos;
volatile float res_neg_pos;
volatile int res1_tmp;
volatile int res2_tmp;
volatile int res1_pos;
volatile int res2_pos;
volatile int res1_neg;
@ -65,13 +46,17 @@ volatile int res_avg_tmp;
volatile int followe;
// pid para
float pid_p;
float pid_i;
float pid_d;
float pid_i_limit;
float pid_error_limit;
float pid_output_limit;
float pid_periode;
struct pid_para{
float p;
float i;
float d;
float ff0;
float ff1;
float i_limit;
float error_limit;
float output_limit;
float periode;
} pid_pp, pid_fp;
// pid cur para
float cur_p;
@ -79,31 +64,26 @@ float cur_min;
float cur_max;
// pid storage
float pid_error;
float pid_error_old;
float pid_error_old_old;
float pid_error_sum;
float pid_in;
float pid_in_old;
float pid_in_old_old;
float pos_error_sum;
float pos_p;
float pos_i;
float freq;
float step;
int do_pid;
float res_pos_hist0;
float res_pos_hist1;
float res_pos_hist2;
volatile float v;
volatile float g;
struct pid_mem{
float error_old;
float error_sum;
float in_old;
float in_old_old;
} pid_pm, pid_fm;
volatile float mot_pos;
volatile float mot_vel;
volatile float vel;
volatile float mag_pos;
volatile float mag_offset;
volatile float current_scale;
volatile float res_pos;
volatile float res_pos_old;
volatile float res_pos_pos;
volatile float res_neg_pos;
volatile int do_pid;
volatile float do_pos;
volatile int dacpos;
@ -147,94 +127,121 @@ float mod(float a){
}
void output_pwm(){
float ctr = mod((mag_pos + res_offset) * pole_count);
float ctr = mod((mag_pos + res_offset + mag_offset) * pole_count);
TIM4->CCR1 = (sine(ctr + offseta) * pwm_scale * current_scale + 1.0) * mag_res / 2.0;
TIM4->CCR2 = (sine(ctr + offsetb) * pwm_scale * current_scale + 1.0) * mag_res / 2.0;
TIM4->CCR3 = (sine(ctr + offsetc) * pwm_scale * current_scale + 1.0) * mag_res / 2.0;
}
void init_pid(){
max_mag_diff = 2*pi/pole_count/4;
// pid para
pid_periode = 1/10000.0;
pid_p = 3.0;
pid_i = 0.004;//;
pid_d = 0.001;//;
pid_i_limit = maxdiff * 100;
pid_error_limit = DEG(90);
pid_output_limit = maxdiff;
pos_error_sum = 0;
pos_p = 2;
pos_i = 0.2;
// pid_p
pid_pp.periode = 1/2000.0;
pid_pp.p = 0.5;
pid_pp.i = 0.0;
pid_pp.d = 0.0;
pid_pp.ff0 = 0.0;
pid_pp.ff1 = 0.0;
pid_pp.i_limit = 11500 / 60.0 * 2.0 * pi;
pid_pp.output_limit = 11500 / 60.0 * 2.0 * pi;
// pid_f
pid_fp.periode = 1/2000.0;
pid_fp.p = 0.0;
pid_fp.i = 0.0;
pid_fp.d = 0.0;
pid_fp.ff0 = 0.0;
pid_fp.ff1 = 0.0;
pid_fp.i_limit = max_mag_diff;
pid_fp.output_limit = max_mag_diff;
// pid cur para
cur_p = 1 / (maxdiff);
cur_p = 1 / (max_mag_diff);
cur_min = 0.1;
cur_max = 1;
// pid storage
pid_error = 0;
pid_error_old = 0;
pid_error_old_old = 0;
pid_error_sum = 0;
pid_in = 0;
pid_in_old = 0;
pid_in_old_old = 0;
pid_pm.error_old = 0;
pid_pm.error_sum = 0;
pid_pm.in_old = 0;
pid_pm.in_old = 0;
pid_fm.error_old = 0;
pid_fm.error_sum = 0;
pid_fm.in_old = 0;
pid_fm.in_old = 0;
}
void rotate_mag(){ // rotate mag_pos
// in mag_pos, v
// out mag_pos
mag_pos += vel/10000;
mag_pos = mod(mag_pos);
}
void pid(){ // strom/kraft pid
float ctr_mag = 0;
float ctr_cur = 0;
float error_der = 0;
void pid_f(){ // calc force / mag_offset
// in v, res_vel
// out mag_offset
float res_vel = minus(res_pos, res_pos_old) / pid_fp.periode;
float error = minus(vel, res_vel);
//float error = minus(mot_pos, res_pos);
float error_d = error - pid_fm.error_old;
float in_d = minus(vel, pid_fm.in_old);
//float in_d = minus(res_pos, pid_fm.in_old);
float in_dd = minus(in_d, minus(pid_fm.in_old, pid_fm.in_old_old));
mag_pos = res_pos;
//mot_pos = DEG((float)UB_ENCODER_TIM3_ReadPos()/2000.0*360.0);
pid_fm.error_old = error;
pid_fm.error_sum = CLAMP((pid_fm.error_sum + error), -pid_fp.i_limit, pid_fp.i_limit);
pid_fm.in_old_old = pid_fm.in_old;
//pid_fm.in_old = res_pos;
pid_fm.in_old = vel;
// calc pid storage
pid_in_old_old = pid_in_old;
pid_in_old = pid_in;
pid_in = mot_pos;
mag_offset = CLAMP(mod(
pid_fp.p * error +
pid_fp.i * pid_fp.periode * pid_fm.error_sum +
pid_fp.d / pid_fp.periode * error_d +
pid_fp.ff0 * in_d +
pid_fp.ff1 * in_dd
), -pid_fp.output_limit, pid_fp.output_limit);
pid_error_old_old = pid_error_old;
pid_error_old = pid_error;
pid_error = minus(mag_pos, minus(res_pos, mag_pos_offset));
//current_scale = CLAMP(ABS((cur_p * cur_p * cur_p * cur_p * mag_offset * mag_offset * mag_offset * mag_offset)) * (cur_max - cur_min) + cur_min, 0, 1);
current_scale = CLAMP(ABS((cur_p * mag_offset)) * (cur_max - cur_min) + cur_min, 0, 1);
}
void pid_p(){ // calc v
// in res_pos, mot_pos
// out v
float error = minus(mot_pos, res_pos);
float error_d = error - pid_pm.error_old;
float in_d = minus(mot_pos, pid_pm.in_old);
float in_dd = minus(in_d, minus(pid_pm.in_old, pid_pm.in_old_old));
pid_pm.error_old = error;
pid_pm.error_sum = CLAMP((pid_pm.error_sum + error), -pid_pp.i_limit, pid_pp.i_limit);
pid_pm.in_old_old = pid_pm.in_old;
pid_pm.in_old = mot_pos;
if(ABS(pid_error) > pid_error_limit){//schleppfehler
followe = YES;
}
// calc pid
pid_error_sum = CLAMP((pid_error_sum + pid_error), -pid_i_limit, pid_i_limit);
error_der = pid_error - pid_error_old;
ctr_mag = CLAMP((
pid_p * pid_error +
pid_i * pid_periode * pid_error_sum +
pid_d / pid_periode * error_der
), -pid_output_limit, pid_output_limit);
//ctr_mag = CLAMP((pid_p * pid_error), -pid_output_limit, pid_output_limit);
ctr_cur = (cur_p * cur_p * cur_p * cur_p * ctr_mag * ctr_mag * ctr_mag * ctr_mag) * (cur_max - cur_min) + cur_min;
//ctr_cur = 0.5;
// output pid
mag_pos += CLAMP(ctr_mag, -maxdiff, maxdiff);
current_scale = CLAMP(ABS(ctr_cur), 0, 1);
mag_pos = mod(mag_pos);
if(do_pos){
vel =
pid_pp.p * error +
pid_pp.i * pid_pp.periode * pid_pm.error_sum +
pid_pp.d / pid_pp.periode * error_d +
pid_pp.ff0 * in_d +
pid_pp.ff1 * in_dd
;
}
else{
vel = mot_vel;
}
}
void TIM2_IRQHandler(void){//PWM int handler, 10KHz
TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
//GPIO_SetBits(GPIOD,GPIO_Pin_11);
//mag_pos += 1/10000.0 * DEG(30);
//current_scale = 0.3;
//mot_pos = mod(mot_pos);
if(do_pid){
pid();
}
rotate_mag();
output_pwm();
//
}
@ -259,60 +266,46 @@ void TIM7_IRQHandler(void){//DAC int handler
res_pos_pos = atan2f(res1_pos, res2_pos);
res1_pos = 0;
res2_pos = 0;
//pid();
//output_pwm();
}
if(dacpos == (read_neg + read_w + 1) % 32){
res_neg_pos = atan2f(res1_neg, res2_neg);
//res_pos = observer(res_pos_pos, res_neg_pos);
res_pos = res_pos_pos + minus(res_neg_pos, res_pos_pos) / 2.0;
float error = minus(mot_pos, res_pos);
pos_error_sum = CLAMP((pos_error_sum + error), -pid_i_limit, pid_i_limit);
mag_pos_offset = pos_p * error + pos_i * pos_error_sum;
//res_pos = res_pos_pos;
res1_neg = 0;
res2_neg = 0;
//pid();
//output_pwm();
res_pos_old = res_pos;
res_pos = res_pos_pos + minus(res_neg_pos, res_pos_pos) / 2.0;
pid_p();
pid_f();
}
}
void ADC_IRQHandler(void)
{
int t1, t2, t3;
int t1, t2;
while(!ADC_GetFlagStatus(ADC2, ADC_FLAG_EOC));
//while(!ADC_GetFlagStatus(ADC3, ADC_FLAG_EOC));
ADC_ClearITPendingBit(ADC1, ADC_IT_EOC);
GPIO_SetBits(GPIOD,GPIO_Pin_11);
t1 = ADC_GetConversionValue(ADC1);
t2 = ADC_GetConversionValue(ADC2);
//t3 = ADC_GetConversionValue(ADC3);
res_avg_tmp += t1+t2;
t1 -= res_avg;
t2 -= res_avg;
//if(t1 < 930 && t1 > -930 && t2 < 930 && t2 > -930){
float max = 930;
if(dacpos >= read_pos - read_w && dacpos <= read_pos + read_w){
//if(t1 > -max & t1 < max & t2 > -max & t2 < max){
res1_pos += t1;
res2_pos += t2;
//}
res1_pos += t1;
res2_pos += t2;
}
else if(dacpos >= read_neg - read_w || dacpos <= (read_neg + read_w) % 32){
//if(t1 > -max & t1 < max & t2 > -max & t2 < max){
res1_neg -= t1;
res2_neg -= t2;
//}
res1_neg -= t1;
res2_neg -= t2;
}
//}
GPIO_ResetBits(GPIOD,GPIO_Pin_11);
}
@ -331,10 +324,11 @@ void findoff(void){
}
int main(void)
{
{
res_avg = 2051;
res_avg_tmp = 0;
do_pid = YES;
do_pos = YES;
dacpos = 31;
init_pid();
param_init();
@ -342,17 +336,13 @@ int main(void)
mag_pos = 0;
mot_pos = 0;
followe = NO;
current_scale = 1;
freq = 0;
step = freq / 10000 * pi * 2.0;
current_scale = 0.3;
mot_vel = 0;
/* TIM4 enable counter */
res_pos = -10;
while (res_pos == -10){
}
res_offset = 2.4885;
res_pos_hist0 = res_pos;
res_pos_hist1 = res_pos;
res_pos_hist2 = res_pos;
mot_pos = res_pos;
TIM_Cmd(TIM4, ENABLE);//PWM
TIM_Cmd(TIM2, ENABLE);//int
@ -381,13 +371,19 @@ int main(void)
float p2 = 0;
int scanf_ret = 0;
ansistate = init;
register_float('p', &pid_p);
register_float('i', &pid_i);
register_float('d', &pid_d);
register_float('p', &pid_pp.p);
register_float('i', &pid_pp.i);
register_float('d', &pid_pp.d);
register_float('a', &pid_fp.p);
register_float('b', &pid_fp.i);
register_float('c', &pid_fp.d);
register_float('m', &mot_pos);
register_float('a', &pos_p);
register_float('b', &pos_i);
register_float('p', &do_pos);
register_float('v', &mot_vel);
float res_vel = 0;
float verror = 0;
if(!do_pid)
findoff();
@ -396,8 +392,11 @@ int main(void)
//printf_("p1 = %f, p2 = %f, n1 = %f, n2 = %f\n", max_res1, max_res2, min_res1, min_res2);
//printf_("p = %f, n = %f\rn", res_pos_pos, res_neg_pos);
//printf_("error = %f\tmot = %f\tcur = %f\n", minus(mot_pos, res_pos), mot_pos, current_scale);
Delay(100000);
printf_("res_avg = %i\t\n", res_avg);
Delay(1000000);
res_vel = minus(res_pos, res_pos_old) / pid_fp.periode;
verror = minus(vel, res_vel);
printf_("pe = %f\tmo = %f\tc = %f\tv = %f\trv = %f\tve = %f\t \r", minus(mot_pos, res_pos), mag_offset, current_scale, vel, res_vel, verror);
if(stlinky_todo(&g_stlinky_term) == 0 && obuf_pos > 0){
stlinky_tx(&g_stlinky_term, outbuf, obuf_pos);
obuf_pos = 0;