diff --git a/shared/comps/rl.c b/shared/comps/rl.c new file mode 100644 index 00000000..c82567ac --- /dev/null +++ b/shared/comps/rl.c @@ -0,0 +1,133 @@ +#include "commands.h" +#include "hal.h" +#include "math.h" +#include "defines.h" +#include "angle.h" + +HAL_COMP(rl); + +HAL_PIN(ra); +HAL_PIN(rb); +HAL_PIN(ld); +HAL_PIN(lq); +HAL_PIN(la); +HAL_PIN(lb); + +HAL_PIN(max_cur); +HAL_PIN(udc); +HAL_PIN(start); +HAL_PIN(state); + +HAL_PIN(ki); +HAL_PIN(time); +HAL_PIN(t); + +HAL_PIN(ia_fb); +HAL_PIN(ib_fb); +HAL_PIN(ua_fb); +HAL_PIN(ub_fb); +HAL_PIN(ua); +HAL_PIN(ub); + +struct rl_ctx_t{ + uint32_t state; + float error_sum; + float time; + float ra; + float rb; +}; + +static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) { + struct rl_ctx_t * ctx = (struct rl_ctx_t *)ctx_ptr; + struct rl_pin_ctx_t *pins = (struct rl_pin_ctx_t *)pin_ptr; + + PIN(time) = 1.0; + PIN(ki) = 100.0; + PIN(max_cur) = 1.0; + ctx->ra = 0.0; + ctx->rb = 0.0; + ctx->state = 0; + ctx->time = 0.0; + ctx->error_sum = 0.0; +} + +static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) { + struct rl_ctx_t * ctx = (struct rl_ctx_t *)ctx_ptr; + struct rl_pin_ctx_t *pins = (struct rl_pin_ctx_t *)pin_ptr; + + switch(ctx->state){ + case 0: + if(PIN(start) > 0.0){ + ctx->state = 1; + ctx->error_sum = 0.0; + ctx->time = 0.0; + } + PIN(ua) = 0.0; + PIN(ub) = 0.0; + break; + + case 1: // measure ra + ctx->time += period; + ctx->error_sum += (PIN(max_cur) - PIN(ia_fb)) * period * PIN(ki); + PIN(ua) = ctx->error_sum; + + if(ABS(PIN(ia_fb) > 0.1)){ + ctx->ra = ctx->ra * 0.95 + PIN(ua_fb) / PIN(ia_fb) * 0.05; + } + + if(ctx->time >= PIN(time)){ + ctx->time = 0.0; + ctx->state = 2; + ctx->error_sum = 0.0; + PIN(ua) = 0.0; + PIN(ub) = 0.0; + } + break; + + case 2: // measure rb + ctx->time += period; + ctx->error_sum += (PIN(max_cur) - PIN(ib_fb)) * period * PIN(ki); + PIN(ub) = ctx->error_sum; + + if(ABS(PIN(ib_fb) > 0.1)){ + ctx->rb = ctx->rb * 0.95 + PIN(ub_fb) / PIN(ib_fb) * 0.05; + } + + if(ctx->time >= PIN(time)){ + ctx->time = 0.0; + ctx->state = 3; + ctx->error_sum = 0.0; + PIN(ua) = 0.0; + PIN(ub) = 0.0; + } + break; + + default: + if(PIN(start) <= 0.0){ + ctx->state = 0; + } + ctx->time = 0.0; + PIN(ua) = 0.0; + PIN(ub) = 0.0; + break; + } + + PIN(state) = ctx->state; + PIN(t) = ctx->time; + PIN(ra) = ctx->ra; + PIN(rb) = ctx->rb; +} + +hal_comp_t rl_comp_struct = { + .name = "rl", + .nrt = 0, + .rt = rt_func, + .frt = 0, + .nrt_init = nrt_init, + .rt_start = 0, + .frt_start = 0, + .rt_stop = 0, + .frt_stop = 0, + .ctx_size = 0, + .pin_count = sizeof(struct rl_pin_ctx_t) / sizeof(struct hal_pin_inst_t), +}; diff --git a/shared/f3hw.h b/shared/f3hw.h index 1ccfe298..708266da 100644 --- a/shared/f3hw.h +++ b/shared/f3hw.h @@ -15,11 +15,11 @@ #define PWM_W TIM8->CCR1 #define PWM_DEADTIME 0 -#define PWM_FREQ 50000 -#define PWM_RES (72000000 * 2 / PWM_FREQ / 2) // 1440 -#define RT_FREQ 5000 -#define ADC_OVER 4 -#define ADC_COUNT (PWM_FREQ * 2 * ADC_OVER / RT_FREQ) // 80 +#define PWM_FREQ 100000.0 +#define PWM_RES (72000000.0 * 2.0 / PWM_FREQ / 2.0) +#define RT_FREQ 5000.0 +#define ADC_OVER (PWM_FREQ * 2 / RT_FREQ) +#define ADC_COUNT (ADC_OVER * 4) //io board #define USB_CONNECT_PIN GPIO_PIN_2 diff --git a/shared/hal.c b/shared/hal.c index 4339f453..03fc1c8e 100644 --- a/shared/hal.c +++ b/shared/hal.c @@ -160,6 +160,8 @@ void hal_term_print_info(char *ptr) { printf("HAL_MAX_COMPS %lu/%i\n", hal.comp_inst_count, HAL_MAX_COMPS); printf("HAL_MAX_PINS %lu/%i\n", hal.pin_inst_count, HAL_MAX_PINS); printf("HAL_MAX_CTX %lu/%i\n", hal.ctx_count, HAL_MAX_CTX); + printf("hal struct start %lu\n", (uint32_t)&hal); + printf("hal struct size %lu\n", sizeof(hal)); hal_term_print_state(); // printf("link errors %lu\n", hal.link_errors); diff --git a/stm32f303/Makefile b/stm32f303/Makefile index a024b3c8..266c48ed 100644 --- a/stm32f303/Makefile +++ b/stm32f303/Makefile @@ -29,6 +29,7 @@ SOURCES += stm32f303/src/version.c SOURCES += stm32f303/src/hal_tbl.c +CFLAGS += -DHAL_MAX_PINS=512 CFLAGS += -DHAL_MAX_CTX=1024 COMPS += stm32f303/src/comps/hv.c # COMPS += stm32f303/src/comps/hvdc.c @@ -47,6 +48,8 @@ COMPS += shared/comps/pole.c COMPS += shared/comps/map.c # COMPS += shared/comps/rev.c COMPS += shared/comps/vel.c +# COMPS += shared/comps/trc.c +COMPS += shared/comps/rl.c # COMPS += shared/comps/hal_test.c # COMPS += shared/comps/dc.c COMPS += shared/comps/ypid.c diff --git a/stm32f303/STM32F303CBTx_FLASH.ld b/stm32f303/STM32F303CBTx_FLASH.ld index 9ec74fa5..94cfbe7f 100644 --- a/stm32f303/STM32F303CBTx_FLASH.ld +++ b/stm32f303/STM32F303CBTx_FLASH.ld @@ -54,7 +54,7 @@ ENTRY(Reset_Handler) /* Highest address of the user mode stack */ _estack = 0x20008000; /* end of RAM */ /* Generate a link error if heap and stack don't fit into RAM */ -_Min_Heap_Size = 0x200; /* required amount of heap */ +_Min_Heap_Size = 0x800; /* required amount of heap */ _Min_Stack_Size = 0x400; /* required amount of stack */ /* Specify the memory areas */ diff --git a/stm32f303/inc/opamp.h b/stm32f303/inc/opamp.h index 3e46a5ab..8f523aaf 100644 --- a/stm32f303/inc/opamp.h +++ b/stm32f303/inc/opamp.h @@ -70,6 +70,7 @@ extern void Error_Handler(void); void MX_OPAMP1_Init(void); void MX_OPAMP2_Init(void); void MX_OPAMP3_Init(void); +void MX_OPAMP4_Init(void); /* USER CODE BEGIN Prototypes */ diff --git a/stm32f303/src/comps/hv.c b/stm32f303/src/comps/hv.c index b7747066..a0a681ec 100644 --- a/stm32f303/src/comps/hv.c +++ b/stm32f303/src/comps/hv.c @@ -12,6 +12,9 @@ HAL_COMP(hv); HAL_PIN(a); HAL_PIN(b); +HAL_PIN(a_fb); +HAL_PIN(b_fb); + //dclink input HAL_PIN(udc); @@ -58,43 +61,30 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ float udc = MAX(PIN(udc), 0.1); //convert voltages to PWM output compare values - int32_t a = (int32_t)(LIMIT(PIN(a), udc) / udc * PWM_RES / 2) + PWM_RES / 2; - int32_t b = (int32_t)(LIMIT(PIN(b), udc) / udc * PWM_RES / 2) + PWM_RES / 2; + int32_t a = (int32_t)(PIN(a) / udc * PWM_RES / 2.0) + PWM_RES / 2; + int32_t b = (int32_t)(PIN(b) / udc * PWM_RES / 2.0) + PWM_RES / 2; //convert on and off times to PWM output compare values - int32_t min_on = (int32_t)(PWM_RES * RT_FREQ * PIN(min_on) + 0.5); + int32_t min = (int32_t)(PWM_RES * RT_FREQ * PIN(min_on) + 0.5); int32_t min_off = (int32_t)(PWM_RES * RT_FREQ * PIN(min_off) + 0.5); - if((a > 0 && a < min_on) || (b > 0 && b < min_on)) { - a += min_on; - b += min_on; - } + a = CLAMP(a, min, PWM_RES - min_off); + b = CLAMP(b, min, PWM_RES - min_off); - if((a > PWM_RES - min_off) || (b > PWM_RES - min_off)) { - a -= min_off; - b -= min_off; - } - - // #ifdef PWM_INVERT - // TIM1->CCR1 = 1440-CLAMP(a, 0, 1440 - min_off); - // TIM1->CCR2 = 1440-CLAMP(b, 0, 1440 - min_off); - // #else - TIM1->CCR1 = CLAMP(a, 0, PWM_RES - min_off); - TIM1->CCR2 = CLAMP(b, 0, PWM_RES - min_off); - GPIOC->BSRR |= GPIO_PIN_15 << 16; + TIM1->CCR1 = a; + TIM1->CCR2 = b; + // GPIOC->BSRR |= GPIO_PIN_15 << 16; - // #endif + PIN(a_fb) = (a - PWM_RES / 2.0) * 2.0 * udc / PWM_RES; + PIN(b_fb) = (b - PWM_RES / 2.0) * 2.0 * udc / PWM_RES; - if(PIN(hv_temp) < 100.0 && PIN(en) > 0.0) { + if(PIN(udc) < 52.0 && PIN(hv_temp) < 100.0 && PIN(en) > 0.0) { HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, PIN(enb) > 0.0 ? GPIO_PIN_SET : GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, PIN(ena) > 0.0 ? GPIO_PIN_SET : GPIO_PIN_RESET); } else { HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET); } - - //TODO: check enable timing on fault pin - // PIN(fault) = HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_7); } hal_comp_t hv_comp_struct = { diff --git a/stm32f303/src/comps/io.c b/stm32f303/src/comps/io.c index 8734d017..771ea54d 100644 --- a/stm32f303/src/comps/io.c +++ b/stm32f303/src/comps/io.c @@ -57,8 +57,8 @@ struct adc_34_t { uint16_t shunt_b3; }; -volatile struct adc_12_t adc_12_buf[ADC_COUNT / ADC_OVER]; -volatile struct adc_34_t adc_34_buf[ADC_COUNT / ADC_OVER]; +volatile struct adc_12_t adc_12_buf[(uint32_t)ADC_OVER]; +volatile struct adc_34_t adc_34_buf[(uint32_t)ADC_OVER]; struct io_ctx_t { float u_offset; @@ -152,10 +152,10 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ uint32_t ian = 0; uint32_t ibp = 0; uint32_t ibn = 0; - uint32_t ip = 0; - uint32_t in = 0; + // uint32_t ip = 0; + // uint32_t in = 0; - for(int i = 0; i < ADC_COUNT / ADC_OVER / 2; i++) { + for(int i = 0; i < ADC_OVER / 2; i++) { dc_link += adc_12_buf[2 * i].dc_link + adc_12_buf[2 * i + 1].dc_link; hv_temp += adc_12_buf[2 * i].hv_temp + adc_12_buf[2 * i + 1].hv_temp; in0 += adc_12_buf[2 * i].in0 + adc_12_buf[2 * i + 1].in0; @@ -166,25 +166,25 @@ static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_ iap += adc_34_buf[2 * i + 1].shunt_a0;// + adc_34_buf[2 * i + 1].shunt_a1 + adc_34_buf[2 * i + 1].shunt_a2 + adc_34_buf[2 * i + 1].shunt_a3; ibn += adc_34_buf[2 * i].shunt_b0;// + adc_34_buf[2 * i].shunt_b1 + adc_34_buf[2 * i].shunt_b2 + adc_34_buf[2 * i].shunt_b3; ibp += adc_34_buf[2 * i + 1].shunt_b0;// + adc_34_buf[2 * i + 1].shunt_b1 + adc_34_buf[2 * i + 1].shunt_b2 + adc_34_buf[2 * i + 1].shunt_b3; - in += adc_12_buf[2 * i].shunt_low0;// + adc_12_buf[2 * i].shunt_low1; - ip += adc_12_buf[2 * i + 1].shunt_low0;// + adc_12_buf[2 * i + 1].shunt_low1; + // in += adc_12_buf[2 * i].shunt_low0;// + adc_12_buf[2 * i].shunt_low1; + // ip += adc_12_buf[2 * i + 1].shunt_low0;// + adc_12_buf[2 * i + 1].shunt_low1; } - PIN(hv_temp) = r2temp(HV_R(ADC(hv_temp * ADC_OVER / ADC_COUNT))); - PIN(dc_link) = dc_link * 3.3 / ARES * (20.0 + 1.0) / 1.0 * ADC_OVER / ADC_COUNT; - PIN(bemf0) = bemf0 * 3.3 / ARES * (20.0 + 1.0) / 1.0 * ADC_OVER / ADC_COUNT; - PIN(bemf1) = bemf1 * 3.3 / ARES * (20.0 + 1.0) / 1.0 * ADC_OVER / ADC_COUNT; - PIN(in0) = in0 * 3.3 / ARES * (10.0 + 1.5) / 1.5 * ADC_OVER / ADC_COUNT; - PIN(in1) = in1 * 3.3 / ARES * (10.0 + 1.5) / 1.5 * ADC_OVER / ADC_COUNT; + PIN(hv_temp) = r2temp(HV_R(ADC(hv_temp / ADC_OVER))); + PIN(dc_link) = ADC(dc_link / ADC_OVER) * (20.0 + 1.0) / 1.0; + PIN(bemf0) = ADC(bemf0 / ADC_OVER) * (20.0 + 1.0) / 1.0; + PIN(bemf1) = ADC(bemf1 / ADC_OVER) * (20.0 + 1.0) / 1.0; + PIN(in0) = ADC(in0 / ADC_OVER) * (10.0 + 1.5) / 1.5; + PIN(in1) = ADC(in1 / ADC_OVER) * (10.0 + 1.5) / 1.5; - PIN(iap) = AMP(iap * 2.0 * ADC_OVER / ADC_COUNT / 1.0, 8.0); - PIN(ian) = AMP(ian * 2.0 * ADC_OVER / ADC_COUNT / 1.0, 8.0); - PIN(ibp) = AMP(ibp * 2.0 * ADC_OVER / ADC_COUNT / 1.0, 8.0); - PIN(ibn) = AMP(ibn * 2.0 * ADC_OVER / ADC_COUNT / 1.0, 8.0); - PIN(ip) = ip * ADC_OVER / ADC_COUNT * 3.3 / ARES; - PIN(in) = in * ADC_OVER / ADC_COUNT * 3.3 / ARES; - PIN(ia) = PIN(iap) - PIN(ian); - PIN(ib) = PIN(ibp) - PIN(ibn); + PIN(iap) = AMP(iap * 2.0 / ADC_OVER, 8.0); + PIN(ian) = AMP(ian * 2.0 / ADC_OVER, 8.0); + PIN(ibp) = AMP(ibp * 2.0 / ADC_OVER, 8.0); + PIN(ibn) = AMP(ibn * 2.0 / ADC_OVER, 8.0); + // PIN(ip) = ADC(ip / ADC_OVER); + // PIN(in) = ADC(in / ADC_OVER);; + PIN(ia) = (PIN(iap) - PIN(ian)) / 2.0; + PIN(ib) = (PIN(ibp) - PIN(ibn)) / 2.0; HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, PIN(led) > 0.0 ? GPIO_PIN_SET : GPIO_PIN_RESET); // 0.1u } diff --git a/stm32f303/src/main.c b/stm32f303/src/main.c index 9360727a..de599b87 100644 --- a/stm32f303/src/main.c +++ b/stm32f303/src/main.c @@ -72,7 +72,7 @@ static void MX_TIM1_Init(void) { htim1.Instance = TIM1; htim1.Init.Prescaler = 0; htim1.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; - htim1.Init.Period = 1440; + htim1.Init.Period = PWM_RES; htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim1.Init.RepetitionCounter = 0; // htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; @@ -85,7 +85,7 @@ static void MX_TIM1_Init(void) { HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig); sConfigOC.OCMode = TIM_OCMODE_PWM1; - sConfigOC.Pulse = 720; + sConfigOC.Pulse = PWM_RES / 2; sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; @@ -172,8 +172,8 @@ void Error_Handler(void); void DMA1_Channel1_IRQHandler() { - GPIOC->BSRR |= GPIO_PIN_14; - GPIOC->BSRR |= GPIO_PIN_15; + // GPIOC->BSRR |= GPIO_PIN_14; + // GPIOC->BSRR |= GPIO_PIN_15; // __HAL_DMA_CLEAR_FLAG(&hdma1, DMA_FLAG_TC1); DMA1->IFCR = DMA_IFCR_CTCIF1; DMA2->IFCR = DMA_IFCR_CTCIF5; @@ -183,7 +183,7 @@ void DMA1_Channel1_IRQHandler() { // hal_stop(); // hal.hal_state = RT_TOO_LONG; // } - GPIOC->BSRR |= GPIO_PIN_14 << 16; + // GPIOC->BSRR |= GPIO_PIN_14 << 16; } void bootloader(char *ptr) { @@ -232,7 +232,7 @@ int main(void) { HAL_GPIO_WritePin(USB_DISCONNECT_PORT, USB_DISCONNECT_PIN, GPIO_PIN_RESET); #endif - MX_TIM8_Init(); + // MX_TIM8_Init(); MX_TIM1_Init(); MX_ADC1_Init(); MX_ADC2_Init(); @@ -359,7 +359,7 @@ int main(void) { load_comp(comp_by_name("vel")); load_comp(comp_by_name("vel")); load_comp(comp_by_name("ypid")); - load_comp(comp_by_name("trc")); + // load_comp(comp_by_name("rl")); load_comp(comp_by_name("hv")); load_comp(comp_by_name("curpid")); @@ -374,6 +374,7 @@ int main(void) { hal_parse("trc0.rt_prio = 6.5"); hal_parse("curpid0.rt_prio = 7.0"); hal_parse("idq0.rt_prio = 8.0"); + hal_parse("rl0.rt_prio = 8.0"); hal_parse("hv0.rt_prio = 9.0"); hal_parse("term0.rt_prio = 10"); @@ -406,11 +407,11 @@ int main(void) { hal_parse("curpid0.ld = 0.0038"); hal_parse("curpid0.lq = 0.0038"); hal_parse("curpid0.psi = 0.005"); - hal_parse("curpid0.kp = 0.6"); - hal_parse("curpid0.ki = 0.0002"); + hal_parse("curpid0.kp = 1.0"); + hal_parse("curpid0.ki = 0.0005"); hal_parse("curpid0.ff = 1"); hal_parse("curpid0.kind = 0"); - hal_parse("curpid0.max_cur = 4"); + hal_parse("curpid0.max_cur = 3.1"); hal_parse("curpid0.pwm_volt = io0.dc_link"); hal_parse("pole0.p = 50.0"); @@ -435,9 +436,9 @@ int main(void) { hal_parse("ypid0.max_vel = 50"); hal_parse("ypid0.max_acc = 10000"); - hal_parse("ypid0.max_out = 4"); + hal_parse("ypid0.max_out = 3"); hal_parse("ypid0.pos_p = 100"); - hal_parse("ypid0.vel_p = 0.1"); + hal_parse("ypid0.vel_p = 0.05"); hal_parse("ypid0.vel_i = 0.05"); hal_parse("ypid0.vel_ff = 1.0"); @@ -455,6 +456,18 @@ int main(void) { hal_parse("curpid0.en = 1"); hal_parse("hv0.en = 1"); + hal_parse("rl0.ua_fb = hv0.a_fb"); + hal_parse("rl0.ub_fb = hv0.b_fb"); + hal_parse("rl0.ia_fb = io0.ia"); + hal_parse("rl0.ib_fb = io0.ib"); + hal_parse("term0.wave4 = rl0.ra"); + hal_parse("term0.wave5 = rl0.rb"); + hal_parse("term0.wave0 = rl0.t"); + hal_parse("term0.wave1 = rl0.state"); + + // hal_parse("hv0.a = rl0.ua"); + // hal_parse("hv0.b = rl0.ub"); + hal_parse("flashloadconf"); hal_parse("loadconf"); hal_parse("start");