mirror of
https://github.com/rene-dev/stmbl.git
synced 2024-12-26 10:32:09 +00:00
+comps
This commit is contained in:
parent
c56dc11afc
commit
9f4ec2b0db
@ -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);
|
||||
|
@ -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
88
src/pwmout.comp
Normal 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;
|
Loading…
Reference in New Issue
Block a user