1
0
mirror of https://github.com/rene-dev/stmbl.git synced 2024-12-26 10:32:09 +00:00
This commit is contained in:
crinq 2014-11-20 11:46:42 +01:00
parent c56dc11afc
commit 9f4ec2b0db
3 changed files with 93 additions and 3 deletions

View File

@ -543,12 +543,12 @@ int main(void)
*/
unsigned int* mem;
mem = (unsigned int*) 0;
mem = (unsigned int*) SCB->VTOR + TIM5_IRQn;
while(1) // Do not exit
{
Wait(1);
//printf_("tim5: %h vec66: %h\n", (unsigned int)TIM5_IRQHandler, (unsigned int)*(mem));
printf_("tim5: %h vec: %h\n", (unsigned int)TIM5_IRQHandler, (unsigned int)*(mem));
period = 0.001;
//printf_("rt_in_count %i\n", hal.rt_in_func_count);

View File

@ -43,6 +43,7 @@ HAL_PIN(acc_ff) = 0.0;
HAL_PIN(force_p) = 0.0;
HAL_PIN(cur_p) = 0.0;
HAL_PIN(cur_ff) = 0.0;
//HAL_PIN(cur_i) = 0.0;
HAL_PIN(cur_d) = 0.0;
HAL_PIN(cur_ind) = 0.0;
@ -108,6 +109,7 @@ RT_PID(
float forcep = PIN(force_p);
float curp = PIN(cur_p);
float curff = PIN(cur_ff);
float curd = PIN(cur_d);
float curind = PIN(cur_ind);
float curlp = PIN(cur_lp);
@ -152,7 +154,7 @@ RT_PID(
// current -> volt
curerr = curcmd - curfb;
vltcmd = LIMIT(curp * curerr + curd * (curcmd - cur_cmd_old) / period + curind * velfb, vltmax) * curlp + vltcmd * (1 - curlp);
vltcmd = LIMIT(curp * curerr + curff * curcmd + curd * (curcmd - cur_cmd_old) / period + curind * velfb, vltmax) * curlp + vltcmd * (1 - curlp);
cur_cmd_old = curcmd;
// volt -> pwm

88
src/pwmout.comp Normal file
View File

@ -0,0 +1,88 @@
#define PWM_U TIM4->CCR1
#define PWM_V TIM4->CCR2
#define PWM_W TIM4->CCR3
#define PWM_E TIM4->CCR4
COMP(pwmout);
HAL_PIN(u) = 0.0;
HAL_PIN(v) = 0.0;
HAL_PIN(w) = 0.0;
HAL_PIN(enable) = 0.0;
HAL_PIN(volt) = 0.0;
HAL_PIN(pwm_max) = 0.0;
INIT(
/* TIM4 clock enable */
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
/* GPIOD clock enable */
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP ;
GPIO_Init(GPIOD, &GPIO_InitStructure);
GPIO_PinAFConfig(GPIOD, GPIO_PinSource12, GPIO_AF_TIM4);
GPIO_PinAFConfig(GPIOD, GPIO_PinSource13, GPIO_AF_TIM4);
GPIO_PinAFConfig(GPIOD, GPIO_PinSource14, GPIO_AF_TIM4);
GPIO_PinAFConfig(GPIOD, GPIO_PinSource15, GPIO_AF_TIM4);
/* pwm set up, TIM4*/
/* Compute the prescaler value */ // 168MHz/2 / pwm frq / pwm res - 1
uint16_t PrescalerValue = (uint16_t) ((SystemCoreClock /2) / 10000 / 8400) - 1; // = 4
/* Time base configuration */
TIM_TimeBaseStructure.TIM_Period = mag_res;
TIM_TimeBaseStructure.TIM_Prescaler = PrescalerValue;
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_Pulse = 0;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
/* PWM1 Mode configuration: Channel1 */
TIM_OC1Init(TIM4, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable);
/* PWM1 Mode configuration: Channel2 */
TIM_OC2Init(TIM4, &TIM_OCInitStructure);
TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable);
/* PWM1 Mode configuration: Channel4 */
TIM_OC3Init(TIM4, &TIM_OCInitStructure);
TIM_OC3PreloadConfig(TIM4, TIM_OCPreload_Enable);
/* PWM1 Mode configuration: Channel4 */
TIM_OC4Init(TIM4, &TIM_OCInitStructure);
TIM_OC4PreloadConfig(TIM4, TIM_OCPreload_Enable);
);
RT_OUT(
float v = PIN(volt);
float e = PIN(enable);
int min = CLAMP((1 - PIN(pwm_max)) / 2.0, 0.0, 1.0) * 8400;
int max = 8400 - min;
if(v > 0.0 && e > 0.0){
PWM_U = CLAMP(PIN(u) / v * 8400, min, max);
PWM_V = CLAMP(PIN(v) / v * 8400, min, max);
PWM_W = CLAMP(PIN(w) / v * 8400, min, max);
PWM_E = CLAMP(e * 8400, 0, 8400);
}
else{
PWM_U = 0;
PWM_V = 0;
PWM_W = 0;
PWM_E = 0;
}
);
ENDCOMP;