From 198ac7f638cd7f37240fbdd61573fdf66e3dbcb1 Mon Sep 17 00:00:00 2001 From: crinq Date: Mon, 25 Sep 2017 04:08:42 +0200 Subject: [PATCH] curpid working --- shared/comps/dq.c | 18 ++-- shared/comps/map.c | 187 ++++++++++++++++++++++++++++++++++++ shared/comps/pole.c | 32 +++++++ shared/f3hw.h | 10 +- stm32f303/Makefile | 4 +- stm32f303/src/adc.c | 152 +++++++---------------------- stm32f303/src/comps/io.c | 169 +++++++++++++++++++++++--------- stm32f303/src/comps/tle.c | 2 +- stm32f303/src/main.c | 196 +++++++++++++++++++------------------- stm32f303/src/opamp.c | 20 ++-- 10 files changed, 503 insertions(+), 287 deletions(-) create mode 100644 shared/comps/map.c create mode 100644 shared/comps/pole.c diff --git a/shared/comps/dq.c b/shared/comps/dq.c index ef8b70f0..fad68841 100644 --- a/shared/comps/dq.c +++ b/shared/comps/dq.c @@ -28,14 +28,14 @@ static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst // struct dq_ctx_t * ctx = (struct dq_ctx_t *)ctx_ptr; struct dq_pin_ctx_t * pins = (struct dq_pin_ctx_t *)pin_ptr; - float u = PIN(u); - float v = PIN(v); - float w = PIN(w); + // float u = PIN(u); + // float v = PIN(v); + // float w = PIN(w); //clarke transformation - float a = u * 2.0 / 3.0 - v / 3.0 - w / 3.0; - float b = v / M_SQRT3 - w / M_SQRT3; - float y = u / 3.0 + v / 3.0 + w / 3.0; + float a = PIN(a);//u * 2.0 / 3.0 - v / 3.0 - w / 3.0; + float b = PIN(b);//v / M_SQRT3 - w / M_SQRT3; + float y = 0.0;//u / 3.0 + v / 3.0 + w / 3.0; float p = (int)MAX(PIN(polecount), 1.0); float pos = PIN(pos) * p; @@ -48,9 +48,9 @@ static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst float d = a * co + b * si; float q = - a * si + b * co; - PIN(a) = a; - PIN(b) = b; - PIN(y) = y; + // PIN(a) = a; + // PIN(b) = b; + // PIN(y) = y; PIN(d) = d; PIN(q) = q; diff --git a/shared/comps/map.c b/shared/comps/map.c new file mode 100644 index 00000000..4eb071df --- /dev/null +++ b/shared/comps/map.c @@ -0,0 +1,187 @@ +#include "commands.h" +#include "hal.h" +#include "math.h" +#include "defines.h" +#include "angle.h" + +#define POLES 50.0 + +HAL_COMP(map); + +HAL_PIN(pos_in); +HAL_PIN(pos_out); +HAL_PIN(start); +HAL_PIN(freq); +HAL_PIN(over); +HAL_PIN(print); +HAL_PIN(state); +HAL_PIN(counter); +HAL_PIN(index); + + +struct map_ctx_t{ + float map[(int)POLES]; + float rmap[(int)POLES]; + int state; + int index; + int counter; + float value; + float pos; +}; + +static void nrt_init(volatile void * ctx_ptr, volatile hal_pin_inst_t * pin_ptr){ + struct map_ctx_t * ctx = (struct map_ctx_t *)ctx_ptr; + struct map_pin_ctx_t * pins = (struct map_pin_ctx_t *)pin_ptr; + PIN(over) = 500.0; + for(int i = 0; i < POLES; i++){ + ctx->map[i] = 0.0; + ctx->rmap[i] = 0.0; + } +} + + +static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst_t * pin_ptr){ + struct map_ctx_t * ctx = (struct map_ctx_t *)ctx_ptr; + struct map_pin_ctx_t * pins = (struct map_pin_ctx_t *)pin_ptr; + + PIN(state) = ctx->state; + PIN(counter) = ctx->counter; + PIN(index) = ctx->index; + + int index; + float min, max, p, min_p, max_p, k; + + switch(ctx->state){ + case 0: // pipe through + //PIN(pos_out) = PIN(pos_in); + PIN(pos_out) = 0.0; + + if(PIN(start) > 0.0){ + PIN(pos_out) = 0.0; + ctx->counter++; + if(ctx->counter > 1000){ + ctx->state = 1; + ctx->index = 0; + ctx->pos = 0.0; + } + } + break; + + case 1: // move motor + ctx->pos += 2.0 * M_PI * PIN(freq) * period; + PIN(pos_out) = mod(ctx->pos); + + if(ctx->pos >= (float)ctx->index * 2.0 * M_PI / POLES){ + ctx->value = 0.0; + ctx->counter = 0; + ctx->state = 2; + //PIN(pos_out) = mod((float)ctx->index / POLES * 2.0 * M_PI); + } + + if(ctx->index >= POLES){ + ctx->state = 3; + } + break; + + case 2: // measure + ctx->value += PIN(pos_in) / PIN(over); + ctx->counter++; + if(ctx->counter > PIN(over)){ + ctx->map[ctx->index] = mod(ctx->value); + ctx->state = 1; + ctx->index++; + } + break; + + case 3: // remap + break; + + case 4: // map + index = (int)((mod(PIN(pos_in)) + M_PI) / 2.0 / M_PI * POLES); + min = ctx->rmap[index]; + max = ctx->rmap[(index + 1) % (int)POLES]; + p = mod(PIN(pos_in)); + min_p = mod(index * 2.0 * M_PI / POLES); + max_p = mod((index + 1) * 2.0 * M_PI / POLES); + k = minus(p, min_p) / minus(max_p, min_p); + + PIN(pos_out) = mod(min + minus(max, min) * k); + + if(PIN(start) <= 0.0){ + ctx->state = 0; + } + break; + } +} + +static void nrt_func(volatile void * ctx_ptr, volatile hal_pin_inst_t * pin_ptr){ + struct map_ctx_t * ctx = (struct map_ctx_t *)ctx_ptr; + struct map_pin_ctx_t * pins = (struct map_pin_ctx_t *)pin_ptr; + + if(PIN(print) > 0.0){ + PIN(print) = 0.0; + printf("\nmap(mot_pos) -> fb_pos\n"); + printf("index, map[index], pos(index)\n"); + for(int i = 0; i < POLES; i++){ + printf("%i, %f, %f\n", i, ctx->map[i], (float)i / POLES * 2.0 * M_PI); + } + printf("\nrmap(fb_pos) -> mot_pos\n"); + printf("index, map[index], pos(index)\n"); + for(int i = 0; i < POLES; i++){ + printf("%i, %f, %f\n", i, ctx->rmap[i], (float)i / POLES * 2.0 * M_PI); + } + } + + if(ctx->state == 3){ // remap + + float min, max, p; + int min_index, max_index; + + for(int i = 0; i < POLES; i++){ + min = -10.0; + max = 10.0; + min_index = 0; + max_index = 0; + p = (float)i * 2.0 * M_PI / POLES; + + for(int j = 0; j < POLES; j++){ + if(minus(p, ctx->map[j]) <= min && minus(p, ctx->map[j]) >= 0.0){ + min = minus(p, ctx->map[j]); + min_index = j; + } + if(minus(ctx->map[j], p) <= max && minus(ctx->map[j], p) >= 0.0){ + max = minus(ctx->map[j], p); + max_index = j; + } + } + p = mod(p); + min = mod(min); + max = mod(max); + + ctx->rmap[i] = min_index * 2.0 * M_PI / POLES + + minus(max_index * 2.0 * M_PI / POLES, min_index * 2.0 * M_PI / POLES) * min / (min + max); + + //ctx->rmap[i] = min_index * 2.0 * M_PI / POLES * minus(p, min) / minus(max, min) + max_index * 2.0 * M_PI / POLES * minus(max, p) / minus(max, min); + } + + // for(int i = 0; i < POLES; i++){ + // ctx->map[i] = ctx->rmap[i]; + // } + + ctx->state = 4; + } +} + +hal_comp_t map_comp_struct = { + .name = "map", + .nrt = nrt_func, + .rt = rt_func, + .frt = 0, + .nrt_init = nrt_init, + .rt_start = 0, + .frt_start = 0, + .rt_stop = 0, + .frt_stop = 0, + .ctx_size = sizeof(struct map_ctx_t), + .pin_count = sizeof(struct map_pin_ctx_t) / sizeof(struct hal_pin_inst_t), +}; diff --git a/shared/comps/pole.c b/shared/comps/pole.c new file mode 100644 index 00000000..c36e4dfc --- /dev/null +++ b/shared/comps/pole.c @@ -0,0 +1,32 @@ +#include "commands.h" +#include "hal.h" +#include "math.h" +#include "defines.h" +#include "angle.h" + +HAL_COMP(pole); + +HAL_PIN(pos); +HAL_PIN(cpos); +HAL_PIN(p); + +static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst_t * pin_ptr){ + // struct pole_ctx_t * ctx = (struct pole_ctx_t *)ctx_ptr; + struct pole_pin_ctx_t * pins = (struct pole_pin_ctx_t *)pin_ptr; + + PIN(cpos) = mod(PIN(pos) * PIN(p)); +} + +hal_comp_t pole_comp_struct = { + .name = "pole", + .nrt = 0, + .rt = rt_func, + .frt = 0, + .nrt_init = 0, + .rt_start = 0, + .frt_start = 0, + .rt_stop = 0, + .frt_stop = 0, + .ctx_size = 0, + .pin_count = sizeof(struct pole_pin_ctx_t) / sizeof(struct hal_pin_inst_t), +}; diff --git a/shared/f3hw.h b/shared/f3hw.h index 1f7bb94a..d210d8bf 100644 --- a/shared/f3hw.h +++ b/shared/f3hw.h @@ -1,11 +1,11 @@ //stmbl #define AREF 3.338// analog reference voltage -#define VDIVUP 249000.0 * 2.0//HV div pullup R1,R12 -#define VDIVDOWN 3900.0//HV div pulldown R2,R9 -#define SHUNT 0.003//shunt -#define SHUNT_PULLUP 15000.0 -#define SHUNT_SERIE 470.0 +#define VDIVUP 20000.0 * 2.0//HV div pullup R1,R12 +#define VDIVDOWN 1000.0//HV div pulldown R2,R9 +#define SHUNT 0.013//shunt +#define SHUNT_PULLUP 3900.0 +#define SHUNT_SERIE 120.0 #define LED_Pin GPIO_PIN_13 #define LED_GPIO_Port GPIOC diff --git a/stm32f303/Makefile b/stm32f303/Makefile index fd3c1ec6..f217849e 100644 --- a/stm32f303/Makefile +++ b/stm32f303/Makefile @@ -43,7 +43,9 @@ COMPS += shared/comps/curpid.c COMPS += shared/comps/svm.c COMPS += shared/comps/dq.c COMPS += shared/comps/idq.c -# COMPS += shared/comps/rev.c +COMPS += shared/comps/pole.c +COMPS += shared/comps/map.c +COMPS += shared/comps/rev.c # COMPS += shared/comps/vel.c # COMPS += shared/comps/hal_test.c # COMPS += shared/comps/dc.c diff --git a/stm32f303/src/adc.c b/stm32f303/src/adc.c index 770fb626..d18a61e9 100644 --- a/stm32f303/src/adc.c +++ b/stm32f303/src/adc.c @@ -69,9 +69,9 @@ void MX_ADC1_Init(void) hadc1.Init.DiscontinuousConvMode = DISABLE; hadc1.Init.NbrOfDiscConversion = 1; hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING; - hadc1.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T8_TRGO; + hadc1.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T1_TRGO2; hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc1.Init.NbrOfConversion = 6; + hadc1.Init.NbrOfConversion = 4; hadc1.Init.DMAContinuousRequests = DISABLE; hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV; hadc1.Init.LowPowerAutoWait = DISABLE; @@ -81,10 +81,10 @@ void MX_ADC1_Init(void) Error_Handler(); } - sConfig.Channel = ADC_CHANNEL_3; // pa2 = opamp1_out = iw (3) + sConfig.Channel = ADC_CHANNEL_1; // pa0 = dclink sConfig.Rank = 1; sConfig.SingleDiff = ADC_SINGLE_ENDED; - sConfig.SamplingTime = ADC_SAMPLETIME_19CYCLES_5; + sConfig.SamplingTime = ADC_SAMPLETIME_61CYCLES_5; sConfig.OffsetNumber = ADC_OFFSET_NONE; sConfig.Offset = 0; if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) @@ -93,45 +93,25 @@ void MX_ADC1_Init(void) } sConfig.Rank = 2; + sConfig.Channel = ADC_CHANNEL_4; // pa3 = bemf0 if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) { Error_Handler(); } sConfig.Rank = 3; + sConfig.Channel = ADC_CHANNEL_3; // pa2 = in0 if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) { Error_Handler(); } sConfig.Rank = 4; + sConfig.Channel = ADC_CHANNEL_2; // pa1 = temp if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) { Error_Handler(); } - - sConfig.Rank = 5; - if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) - { - Error_Handler(); - } - - sConfig.Channel = ADC_CHANNEL_4; // pa3 = uw - sConfig.Rank = 6; - sConfig.SamplingTime = ADC_SAMPLETIME_181CYCLES_5; - if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) - { - Error_Handler(); - } - - // sConfig.Channel = ADC_CHANNEL_2; // pa1 = opamp1_in = iw - // sConfig.Rank = 3; - // sConfig.SamplingTime = ADC_SAMPLETIME_61CYCLES_5; - // if (HAL_ADC_ConfigChannel(&hadc3, &sConfig) != HAL_OK) - // { - // Error_Handler(); - // } - } /* ADC2 init function */ void MX_ADC2_Init(void) @@ -149,9 +129,9 @@ void MX_ADC2_Init(void) hadc2.Init.DiscontinuousConvMode = DISABLE; hadc2.Init.NbrOfDiscConversion = 1; hadc2.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING; - hadc2.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T8_TRGO; + hadc2.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T1_TRGO2; hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc2.Init.NbrOfConversion = 6; + hadc2.Init.NbrOfConversion = 4; hadc2.Init.DMAContinuousRequests = DISABLE; hadc2.Init.EOCSelection = ADC_EOC_SINGLE_CONV; hadc2.Init.LowPowerAutoWait = DISABLE; @@ -161,10 +141,10 @@ void MX_ADC2_Init(void) Error_Handler(); } - sConfig.Channel = ADC_CHANNEL_3; // pa6 = opamp2_out = iu (3) + sConfig.Channel = ADC_CHANNEL_3; // pa6 = opamp2_out = shunt low sConfig.Rank = 1; sConfig.SingleDiff = ADC_SINGLE_ENDED; - sConfig.SamplingTime = ADC_SAMPLETIME_19CYCLES_5; + sConfig.SamplingTime = ADC_SAMPLETIME_61CYCLES_5; sConfig.OffsetNumber = ADC_OFFSET_NONE; sConfig.Offset = 0; if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK) @@ -173,44 +153,25 @@ void MX_ADC2_Init(void) } sConfig.Rank = 2; + sConfig.Channel = ADC_CHANNEL_1; // pa4 = bemf1 if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK) { Error_Handler(); } sConfig.Rank = 3; + sConfig.Channel = ADC_CHANNEL_2; // pa5 = in1 if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK) { Error_Handler(); } sConfig.Rank = 4; + sConfig.Channel = ADC_CHANNEL_3; // pa6 = opamp2_out = shunt low if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK) { Error_Handler(); } - - sConfig.Rank = 5; - if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK) - { - Error_Handler(); - } - - sConfig.Channel = ADC_CHANNEL_2; // pa5 = uv - sConfig.Rank = 6; - sConfig.SamplingTime = ADC_SAMPLETIME_181CYCLES_5; - if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK) - { - Error_Handler(); - } - - // sConfig.Channel = ADC_CHANNEL_4; // pa7 = opamp2_in = iu - // sConfig.Rank = 3; - // sConfig.SamplingTime = ADC_SAMPLETIME_61CYCLES_5; - // if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK) - // { - // Error_Handler(); - // } } /* ADC3 init function */ void MX_ADC3_Init(void) @@ -227,9 +188,9 @@ void MX_ADC3_Init(void) hadc3.Init.DiscontinuousConvMode = DISABLE; hadc3.Init.NbrOfDiscConversion = 1; hadc3.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING; - hadc3.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T8_TRGO; + hadc3.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T1_TRGO2; hadc3.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc3.Init.NbrOfConversion = 6; + hadc3.Init.NbrOfConversion = 4; hadc3.Init.DMAContinuousRequests = DISABLE; hadc3.Init.EOCSelection = ADC_EOC_SINGLE_CONV; hadc3.Init.LowPowerAutoWait = DISABLE; @@ -241,10 +202,10 @@ void MX_ADC3_Init(void) /**Configure Regular Channel */ - sConfig.Channel = ADC_CHANNEL_1; // pb1 = opamp3_out = iv (1) + sConfig.Channel = ADC_CHANNEL_1; // pb1 = opamp3_out = shunt a sConfig.Rank = 1; sConfig.SingleDiff = ADC_SINGLE_ENDED; - sConfig.SamplingTime = ADC_SAMPLETIME_19CYCLES_5; + sConfig.SamplingTime = ADC_SAMPLETIME_61CYCLES_5; sConfig.OffsetNumber = ADC_OFFSET_NONE; sConfig.Offset = 0; if (HAL_ADC_ConfigChannel(&hadc3, &sConfig) != HAL_OK) @@ -269,31 +230,6 @@ void MX_ADC3_Init(void) { Error_Handler(); } - - sConfig.Rank = 5; - if (HAL_ADC_ConfigChannel(&hadc3, &sConfig) != HAL_OK) - { - Error_Handler(); - } - - sConfig.Channel = ADC_CHANNEL_5; // pb13 = uu - sConfig.Rank = 6; - sConfig.SamplingTime = ADC_SAMPLETIME_181CYCLES_5; - if (HAL_ADC_ConfigChannel(&hadc3, &sConfig) != HAL_OK) - { - Error_Handler(); - } - // - // /**Configure Regular Channel - // */ - // sConfig.Channel = ADC_CHANNEL_12; // pb0 = opamp3_in = iv - // sConfig.Rank = 3; - // sConfig.SamplingTime = ADC_SAMPLETIME_61CYCLES_5; - // if (HAL_ADC_ConfigChannel(&hadc3, &sConfig) != HAL_OK) - // { - // Error_Handler(); - // } - } /* ADC4 init function */ void MX_ADC4_Init(void) @@ -311,9 +247,9 @@ void MX_ADC4_Init(void) hadc4.Init.DiscontinuousConvMode = DISABLE; hadc4.Init.NbrOfDiscConversion = 1; hadc4.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING; - hadc4.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T8_TRGO; + hadc4.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T1_TRGO2; hadc4.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc4.Init.NbrOfConversion = 6; + hadc4.Init.NbrOfConversion = 4; hadc4.Init.DMAContinuousRequests = DISABLE; hadc4.Init.EOCSelection = ADC_EOC_SINGLE_CONV; hadc4.Init.LowPowerAutoWait = DISABLE; @@ -323,10 +259,10 @@ void MX_ADC4_Init(void) Error_Handler(); } - sConfig.Channel = ADC_CHANNEL_4; // pb14 = hv_temp + sConfig.Channel = ADC_CHANNEL_3; // pb12 = shunt b sConfig.Rank = 1; sConfig.SingleDiff = ADC_SINGLE_ENDED; - sConfig.SamplingTime = ADC_SAMPLETIME_19CYCLES_5; + sConfig.SamplingTime = ADC_SAMPLETIME_61CYCLES_5; sConfig.OffsetNumber = ADC_OFFSET_NONE; sConfig.Offset = 0; if (HAL_ADC_ConfigChannel(&hadc4, &sConfig) != HAL_OK) @@ -346,26 +282,11 @@ void MX_ADC4_Init(void) Error_Handler(); } - sConfig.Channel = ADC_CHANNEL_5; // pb15 = mot_temp sConfig.Rank = 4; if (HAL_ADC_ConfigChannel(&hadc4, &sConfig) != HAL_OK) { Error_Handler(); } - - sConfig.Rank = 5; - if (HAL_ADC_ConfigChannel(&hadc4, &sConfig) != HAL_OK) - { - Error_Handler(); - } - - sConfig.Channel = ADC_CHANNEL_3; // pb12 = hv - sConfig.Rank = 6; - sConfig.SamplingTime = ADC_SAMPLETIME_181CYCLES_5; - if (HAL_ADC_ConfigChannel(&hadc4, &sConfig) != HAL_OK) - { - Error_Handler(); - } } static uint32_t HAL_RCC_ADC12_CLK_ENABLED=0; @@ -387,11 +308,12 @@ void HAL_ADC_MspInit(ADC_HandleTypeDef* adcHandle) } /**ADC1 GPIO Configuration + PA0 ------> ADC1_IN1 PA1 ------> ADC1_IN2 PA2 ------> ADC1_IN3 PA3 ------> ADC1_IN4 */ - GPIO_InitStruct.Pin = GPIO_PIN_1 | GPIO_PIN_2 | GPIO_PIN_3; + GPIO_InitStruct.Pin = GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2 | GPIO_PIN_3; GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); @@ -412,11 +334,11 @@ void HAL_ADC_MspInit(ADC_HandleTypeDef* adcHandle) } /**ADC2 GPIO Configuration + PA4 ------> ADC2_IN1 PA5 ------> ADC2_IN2 PA6 ------> ADC2_IN3 - PA7 ------> ADC2_IN4 */ - GPIO_InitStruct.Pin = GPIO_PIN_5 | GPIO_PIN_6 | GPIO_PIN_7; + GPIO_InitStruct.Pin = GPIO_PIN_4 | GPIO_PIN_5 | GPIO_PIN_6 | GPIO_PIN_7; GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); @@ -437,11 +359,9 @@ void HAL_ADC_MspInit(ADC_HandleTypeDef* adcHandle) } /**ADC3 GPIO Configuration - PB0 ------> ADC3_IN12 PB1 ------> ADC3_IN1 - PB13 ------> ADC3_IN5 */ - GPIO_InitStruct.Pin = GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_13; + GPIO_InitStruct.Pin = GPIO_PIN_0 | GPIO_PIN_1; GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); @@ -463,10 +383,8 @@ void HAL_ADC_MspInit(ADC_HandleTypeDef* adcHandle) /**ADC4 GPIO Configuration PB12 ------> ADC4_IN3 - PB14 ------> ADC4_IN4 - PB15 ------> ADC4_IN5 */ - GPIO_InitStruct.Pin = GPIO_PIN_12 | GPIO_PIN_14 | GPIO_PIN_15; + GPIO_InitStruct.Pin = GPIO_PIN_12; GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); @@ -494,7 +412,7 @@ void HAL_ADC_MspDeInit(ADC_HandleTypeDef* adcHandle) /**ADC1 GPIO Configuration PA3 ------> ADC1_IN4 */ - HAL_GPIO_DeInit(GPIOA, GPIO_PIN_3); + HAL_GPIO_DeInit(GPIOA, GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2 | GPIO_PIN_3); /* USER CODE BEGIN ADC1_MspDeInit 1 */ @@ -512,9 +430,11 @@ void HAL_ADC_MspDeInit(ADC_HandleTypeDef* adcHandle) } /**ADC2 GPIO Configuration + PA4 ------> ADC2_IN1 PA5 ------> ADC2_IN2 + PA6 ------> ADC2_IN3 */ - HAL_GPIO_DeInit(GPIOA, GPIO_PIN_5); + HAL_GPIO_DeInit(GPIOA, GPIO_PIN_4 | GPIO_PIN_5 | GPIO_PIN_6); /* USER CODE BEGIN ADC2_MspDeInit 1 */ @@ -532,9 +452,9 @@ void HAL_ADC_MspDeInit(ADC_HandleTypeDef* adcHandle) } /**ADC3 GPIO Configuration - PB13 ------> ADC3_IN5 + PB1 ------> ADC3_IN1 */ - HAL_GPIO_DeInit(GPIOB, GPIO_PIN_13); + HAL_GPIO_DeInit(GPIOB, GPIO_PIN_1); /* USER CODE BEGIN ADC3_MspDeInit 1 */ @@ -553,10 +473,8 @@ void HAL_ADC_MspDeInit(ADC_HandleTypeDef* adcHandle) /**ADC4 GPIO Configuration PB12 ------> ADC4_IN3 - PB14 ------> ADC4_IN4 - PB15 ------> ADC4_IN5 */ - HAL_GPIO_DeInit(GPIOB, GPIO_PIN_12|GPIO_PIN_14|GPIO_PIN_15); + HAL_GPIO_DeInit(GPIOB, GPIO_PIN_12); /* USER CODE BEGIN ADC4_MspDeInit 1 */ diff --git a/stm32f303/src/comps/io.c b/stm32f303/src/comps/io.c index 24c79239..27de9845 100644 --- a/stm32f303/src/comps/io.c +++ b/stm32f303/src/comps/io.c @@ -10,23 +10,55 @@ HAL_COMP(io); HAL_PIN(led); -HAL_PIN(iu); -HAL_PIN(iv); -HAL_PIN(iw); - -HAL_PIN(u); -HAL_PIN(v); -HAL_PIN(w); -HAL_PIN(udc); -HAL_PIN(udc_pwm); - -HAL_PIN(hv_temp); -HAL_PIN(mot_temp); HAL_PIN(oc1); HAL_PIN(oc2); +HAL_PIN(ena); +HAL_PIN(enb); -uint32_t adc_12_buf[10]; -uint32_t adc_34_buf[10]; +HAL_PIN(hv_temp); +HAL_PIN(dc_link); +HAL_PIN(bemf0); +HAL_PIN(bemf1); +HAL_PIN(in0); +HAL_PIN(in1); +HAL_PIN(iap); +HAL_PIN(ian); +HAL_PIN(ibp); +HAL_PIN(ibn); +HAL_PIN(ip); +HAL_PIN(in); +HAL_PIN(ia); +HAL_PIN(ib); + +// uint32_t adc_12_buf[80]; +// uint32_t adc_34_buf[80]; + +#pragma pack(1) +struct adc_12_t{ + uint16_t dc_link; + uint16_t shunt_low0; + uint16_t bemf0; + uint16_t bemf1; + uint16_t in0; + uint16_t in1; + uint16_t hv_temp; + uint16_t shunt_low1; +}; + +#pragma pack(1) +struct adc_34_t{ + uint16_t shunt_a0; + uint16_t shunt_b0; + uint16_t shunt_a1; + uint16_t shunt_b1; + uint16_t shunt_a2; + uint16_t shunt_b2; + uint16_t shunt_a3; + uint16_t shunt_b3; +}; + +volatile struct adc_12_t adc_12_buf[20]; +volatile struct adc_34_t adc_34_buf[20]; struct io_ctx_t{ float u_offset; @@ -52,7 +84,7 @@ struct io_ctx_t{ #define VOLT(a) ((a) / (ARES) * (AREF) / (VDIVDOWN) * ((VDIVUP) + (VDIVDOWN))) //#define TEMP(a) (log10f((a) * (AREF) / (ARES) * (TPULLUP) / ((AREF) - (a) * (AREF) / (ARES))) * (-53.0) + 290.0) -#define SHUNT_GAIN 16.0 +#define SHUNT_GAIN 8.0 #define AMP(a, gain) (((a) * AREF / ARES / (gain) - AREF / (SHUNT_PULLUP + SHUNT_SERIE) * SHUNT_SERIE) / (SHUNT * SHUNT_PULLUP) * (SHUNT_PULLUP + SHUNT_SERIE)) @@ -96,16 +128,15 @@ static void nrt_init(volatile void * ctx_ptr, volatile hal_pin_inst_t * pin_ptr) GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); - - HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_SET); - HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_SET); + DMA1_Channel1->CCR &= (uint16_t)(~DMA_CCR_EN); DMA1_Channel1->CPAR = (uint32_t)&(ADC12_COMMON->CDR); DMA1_Channel1->CMAR = (uint32_t)adc_12_buf; - DMA1_Channel1->CNDTR = 6; + DMA1_Channel1->CNDTR = 80; DMA1_Channel1->CCR = DMA_CCR_MINC | DMA_CCR_PL_0 | DMA_CCR_MSIZE_1 | DMA_CCR_PSIZE_1 | DMA_CCR_CIRC; ADC1->CFGR |= ADC_CFGR_DMAEN | ADC_CFGR_DMACFG; + DMA1_Channel1->CCR |= DMA_CCR_TCIE; DMA1_Channel1->CCR |= DMA_CCR_EN; // ADC12_COMMON->CCR |= ADC12_CCR_MDMA_1; @@ -113,7 +144,7 @@ static void nrt_init(volatile void * ctx_ptr, volatile hal_pin_inst_t * pin_ptr) DMA2_Channel5->CCR &= (uint16_t)(~DMA_CCR_EN); DMA2_Channel5->CPAR = (uint32_t)&(ADC34_COMMON->CDR); DMA2_Channel5->CMAR = (uint32_t)adc_34_buf; - DMA2_Channel5->CNDTR = 6; + DMA2_Channel5->CNDTR = 80; DMA2_Channel5->CCR = DMA_CCR_MINC | DMA_CCR_PL_0 | DMA_CCR_MSIZE_1 | DMA_CCR_PSIZE_1 | DMA_CCR_CIRC; ADC3->CFGR |= ADC_CFGR_DMAEN | ADC_CFGR_DMACFG; DMA2_Channel5->CCR |= DMA_CCR_EN; @@ -124,35 +155,81 @@ static void nrt_init(volatile void * ctx_ptr, volatile hal_pin_inst_t * pin_ptr) static void rt_func(float period, volatile void * ctx_ptr, volatile hal_pin_inst_t * pin_ptr){ struct io_ctx_t * ctx = (struct io_ctx_t *)ctx_ptr; struct io_pin_ctx_t * pins = (struct io_pin_ctx_t *)pin_ptr; -TIM1->CCR1 = PIN(oc1) * 1200.0+1200.0; -TIM1->CCR2 = PIN(oc2) * 1200.0+1200.0; - // while(!(DMA1->ISR & DMA_ISR_TCIF1)){} - // while(!(DMA2->ISR & DMA_ISR_TCIF5)){} - - DMA1->IFCR = DMA_IFCR_CTCIF1; - DMA2->IFCR = DMA_IFCR_CTCIF5; - uint32_t a12 = adc_12_buf[0] + adc_12_buf[1] + adc_12_buf[2] + adc_12_buf[3] + adc_12_buf[4]; - uint32_t a34 = adc_34_buf[0] + adc_34_buf[1] + adc_34_buf[2] + adc_34_buf[3] + adc_34_buf[4]; - - if(ctx->u_offset == 0){ - ctx->w_offset = AMP((float)(a12 & 0xFFFF) / 5.0, SHUNT_GAIN); - ctx->u_offset = AMP((float)(a12 >> 16) / 5.0, SHUNT_GAIN); - ctx->v_offset = AMP((float)(a34 & 0xFFFF) / 5.0, SHUNT_GAIN); + // uint32_t dc_link = adc_12_buf[2].dc_link; + // uint32_t hv_temp = adc_12_buf[2].hv_temp; + // uint32_t bemf0 = adc_12_buf[2].bemf0; + // uint32_t bemf1 = adc_12_buf[2].bemf1; + // uint32_t in0 = adc_12_buf[2].in0; + // uint32_t in1 = adc_12_buf[2].in1; + // uint32_t iap = adc_34_buf[2].shunt_a0; + // uint32_t ian = adc_34_buf[3].shunt_a0; + // uint32_t ibp = adc_34_buf[2].shunt_b0; + // uint32_t ibn = adc_34_buf[3].shunt_b0; + // uint32_t ip = adc_12_buf[2].shunt_low0; + // uint32_t in = adc_12_buf[3].shunt_low0; + + uint32_t dc_link = 0; + uint32_t hv_temp = 0; + uint32_t bemf0 = 0; + uint32_t bemf1 = 0; + uint32_t in0 = 0; + uint32_t in1 = 0; + uint32_t iap = 0; + uint32_t ian = 0; + uint32_t ibp = 0; + uint32_t ibn = 0; + uint32_t ip = 0; + uint32_t in = 0; + + for(int i = 0; i < 10; 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; + in1 += adc_12_buf[2 * i].in1 + adc_12_buf[2 * i + 1].in1; + bemf0 += adc_12_buf[2 * i].bemf0 + adc_12_buf[2 * i + 1].bemf0; + bemf1 += adc_12_buf[2 * i].bemf1 + adc_12_buf[2 * i + 1].bemf1; + ian += adc_34_buf[2 * i].shunt_a0 + adc_34_buf[2 * i].shunt_a1 + adc_34_buf[2 * i].shunt_a2 + adc_34_buf[2 * i].shunt_a3; + 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; } + +TIM1->CCR1 = PIN(oc1) * 720.0 + 720.0; +TIM1->CCR2 = PIN(oc2) * 720.0 + 720.0; + + // PIN(dclink) = dc_link / 1.0 * 3.3 / ARES * (10.0 + 10.0 + 1.0) / 1.0; + // PIN(bemf0) = bemf0 / 1.0 * 3.3 / ARES * (10.0 + 10.0 + 1.0) / 1.0; + // PIN(bemf1) = bemf1 / 1.0 * 3.3 / ARES * (10.0 + 10.0 + 1.0) / 1.0; + // PIN(in0) = in0 / 1.0 * 3.3 / ARES * (10.0 + 1.5) / 1.5; + // PIN(in1) = in1 / 1.0 * 3.3 / ARES * (10.0 + 1.5) / 1.5; + PIN(hv_temp) = hv_temp / 20.0 * 3.3 / ARES; + PIN(dc_link) = dc_link / 20.0 * 3.3 / ARES * (20.0 + 1.0) / 1.0; + PIN(bemf0) = bemf0 / 20.0 * 3.3 / ARES * (20.0 + 1.0) / 1.0; + PIN(bemf1) = bemf1 / 20.0 * 3.3 / ARES * (20.0 + 1.0) / 1.0; + PIN(in0) = in0 / 20.0 * 3.3 / ARES * (10.0 + 1.5) / 1.5; + PIN(in1) = in1 / 20.0 * 3.3 / ARES * (10.0 + 1.5) / 1.5; + // PIN(iap) = iap / 40.0 * 3.3 / ARES; + // PIN(ian) = ian / 40.0 * 3.3 / ARES; + // PIN(ibp) = ibp / 40.0 * 3.3 / ARES; + // PIN(ibn) = ibn / 40.0 * 3.3 / ARES; + // PIN(ip) = ip / 20.0 * 3.3 / ARES; + // PIN(in) = in / 20.0 * 3.3 / ARES; + PIN(iap) = AMP(iap / 40.0, 8.0); + PIN(ian) = AMP(ian / 40.0, 8.0); + PIN(ibp) = AMP(ibp / 40.0, 8.0); + PIN(ibn) = AMP(ibn / 40.0, 8.0); + PIN(ip) = ip / 20.0 * 3.3 / ARES; + PIN(in) = in / 20.0 * 3.3 / ARES; + PIN(ia) = PIN(iap) - PIN(ian); + PIN(ib) = PIN(ibp) - PIN(ibn); - PIN(iw) = -AMP((float)(a12 & 0xFFFF) / 5.0, SHUNT_GAIN) + ctx->w_offset; // 1u - PIN(iu) = -AMP((float)(a12 >> 16) / 5.0, SHUNT_GAIN) + ctx->u_offset; - PIN(iv) = -AMP((float)(a34 & 0xFFFF) / 5.0, SHUNT_GAIN) + ctx->v_offset; - PIN(w) = VOLT(adc_12_buf[5] & 0xFFFF) * 0.05 + PIN(w) * 0.95; // 0.6u - PIN(v) = VOLT(adc_12_buf[5] >> 16) * 0.05 + PIN(v) * 0.95; - PIN(u) = VOLT(adc_34_buf[5] & 0xFFFF) * 0.05 + PIN(u) * 0.95; - PIN(udc) = VOLT(adc_34_buf[5] >> 16) * 0.05 + PIN(udc) * 0.95; - PIN(udc_pwm) = PIN(udc) / 2.0; - - PIN(hv_temp) = r2temp(HV_R(ADC(adc_34_buf[0] >> 16))) * 0.01 + PIN(hv_temp) * 0.99; // 5.5u - PIN(mot_temp) = MOT_R(MOT_REF(ADC(adc_34_buf[5] >> 16))); // 1.4u - HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, PIN(led) > 0 ? GPIO_PIN_SET : GPIO_PIN_RESET); // 0.1u + + HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, PIN(led) > 0.0 ? GPIO_PIN_SET : GPIO_PIN_RESET); // 0.1u + 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); } hal_comp_t io_comp_struct = { diff --git a/stm32f303/src/comps/tle.c b/stm32f303/src/comps/tle.c index 0d87fb94..e3cc1fd0 100644 --- a/stm32f303/src/comps/tle.c +++ b/stm32f303/src/comps/tle.c @@ -80,7 +80,7 @@ typedef union{ typedef union{ struct { uint8_t fir_md :2;//Filter Decimation Setting (Update Rate Setting) - uint8_t res :9;//Reserved + uint16_t res :9;//Reserved uint8_t clk_sel :1;//Clock Source Select uint8_t ssc_od :1;//SSC Interface 0: Push-pull 1: Open drain uint8_t dsp_hold :1;//Hold DSPU Operation diff --git a/stm32f303/src/main.c b/stm32f303/src/main.c index 39193d33..da9b5466 100644 --- a/stm32f303/src/main.c +++ b/stm32f303/src/main.c @@ -72,19 +72,21 @@ static void MX_TIM1_Init(void) htim1.Instance = TIM1; htim1.Init.Prescaler = 0; - htim1.Init.CounterMode = TIM_COUNTERMODE_UP; - htim1.Init.Period = 2400; + htim1.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; + htim1.Init.Period = 1440; htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim1.Init.RepetitionCounter = 0; + // htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE; // htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; HAL_TIM_OC_Init(&htim1); sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; - sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET; + sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_UPDATE; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig); + sConfigOC.OCMode = TIM_OCMODE_PWM1; - sConfigOC.Pulse = 1200; + sConfigOC.Pulse = 720; sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; @@ -171,15 +173,18 @@ void Error_Handler(void); /* USER CODE BEGIN 0 */ -void TIM8_UP_IRQHandler(){ - //GPIOA->BSRR |= GPIO_PIN_9; - __HAL_TIM_CLEAR_IT(&htim8, TIM_IT_UPDATE); +void DMA1_Channel1_IRQHandler(){ + // GPIOC->BSRR |= GPIO_PIN_13; + // __HAL_DMA_CLEAR_FLAG(&hdma1, DMA_FLAG_TC1); + DMA1->IFCR = DMA_IFCR_CTCIF1; + DMA2->IFCR = DMA_IFCR_CTCIF5; + hal_run_rt(); - if(__HAL_TIM_GET_FLAG(&htim8, TIM_IT_UPDATE) == SET){ - hal_stop(); - hal.hal_state = RT_TOO_LONG; - } - //GPIOA->BSRR |= GPIO_PIN_9 << 16; + // if(__HAL_DMA_GET_FLAG(&hdma1, DMA_FLAG_TC1) == SET){ + // hal_stop(); + // hal.hal_state = RT_TOO_LONG; + // } + // GPIOC->BSRR |= GPIO_PIN_13 << 16; } void bootloader(char * ptr){ @@ -232,10 +237,19 @@ int main(void) MX_TIM8_Init(); MX_TIM1_Init(); - // MX_ADC1_Init(); - // MX_ADC2_Init(); - // MX_ADC3_Init(); - // MX_ADC4_Init(); + MX_ADC1_Init(); + MX_ADC2_Init(); + MX_ADC3_Init(); + MX_ADC4_Init(); + /* DMA interrupt init */ + /* DMA1_Channel1_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(DMA1_Channel1_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(DMA1_Channel1_IRQn); + /* DMA2_Channel5_IRQn interrupt configuration */ + // HAL_NVIC_SetPriority(DMA2_Channel5_IRQn, 0, 0); + // HAL_NVIC_EnableIRQ(DMA2_Channel5_IRQn); + + // MX_DAC_Init(); // MX_OPAMP1_Init(); MX_OPAMP2_Init(); @@ -258,10 +272,10 @@ int main(void) __HAL_RCC_RTC_ENABLE(); /* USER CODE BEGIN 2 */ - // HAL_ADCEx_Calibration_Start(&hadc1, ADC_SINGLE_ENDED); - // HAL_ADCEx_Calibration_Start(&hadc2, ADC_SINGLE_ENDED); - // HAL_ADCEx_Calibration_Start(&hadc3, ADC_SINGLE_ENDED); - // HAL_ADCEx_Calibration_Start(&hadc4, ADC_SINGLE_ENDED); + HAL_ADCEx_Calibration_Start(&hadc1, ADC_SINGLE_ENDED); + HAL_ADCEx_Calibration_Start(&hadc2, ADC_SINGLE_ENDED); + HAL_ADCEx_Calibration_Start(&hadc3, ADC_SINGLE_ENDED); + HAL_ADCEx_Calibration_Start(&hadc4, ADC_SINGLE_ENDED); // HAL_OPAMP_SelfCalibrate(&hopamp1); HAL_OPAMP_SelfCalibrate(&hopamp2); @@ -288,36 +302,32 @@ int main(void) // GPIO_InitStruct.Pull = GPIO_NOPULL; // GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; // HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); -/* - if (HAL_OPAMP_Start(&hopamp1) != HAL_OK){ - Error_Handler(); - } + if (HAL_OPAMP_Start(&hopamp2) != HAL_OK){ Error_Handler(); } if (HAL_OPAMP_Start(&hopamp3) != HAL_OK){ Error_Handler(); } -*/ - htim8.Instance->CCR1 = 0; - htim8.Instance->CCR2 = 0; - htim8.Instance->CCR3 = 0; + if (HAL_OPAMP_Start(&hopamp4) != HAL_OK){ + Error_Handler(); + } + + // htim8.Instance->CCR1 = 0; + // htim8.Instance->CCR2 = 0; + // htim8.Instance->CCR3 = 0; - htim1.Instance->CCR1 = 1200; - htim1.Instance->CCR2 = 1200; - HAL_TIM_Base_Start(&htim1); - HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); - HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2); - HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_1); - HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_2); + // htim1.Instance->CCR1 = 1200; + // htim1.Instance->CCR2 = 1200; + - // HAL_ADC_Start(&hadc1); - // HAL_ADC_Start(&hadc2); - // HAL_ADC_Start(&hadc3); - // HAL_ADC_Start(&hadc4); - if (HAL_TIM_Base_Start_IT(&htim8) != HAL_OK){ - Error_Handler(); - } + HAL_ADC_Start(&hadc1); + HAL_ADC_Start(&hadc2); + HAL_ADC_Start(&hadc3); + HAL_ADC_Start(&hadc4); + // if (HAL_TIM_Base_Start_IT(&htim8) != HAL_OK){ + // Error_Handler(); + // } #ifndef PWM_INVERT TIM8->RCR = 1;//uptate event foo #endif @@ -340,22 +350,29 @@ int main(void) // Error_Handler(); // } - hal_init(1.0 / 15000.0, 0.0); + hal_init(1.0 / 5000.0, 0.0); // hal load comps load_comp(comp_by_name("term")); load_comp(comp_by_name("sim")); load_comp(comp_by_name("io")); // load_comp(comp_by_name("ls")); - // load_comp(comp_by_name("dq")); + load_comp(comp_by_name("dq")); load_comp(comp_by_name("idq")); load_comp(comp_by_name("tle")); + load_comp(comp_by_name("pole")); + load_comp(comp_by_name("map")); + load_comp(comp_by_name("rev")); // load_comp(comp_by_name("svm")); // load_comp(comp_by_name("hv")); - // load_comp(comp_by_name("curpid")); + load_comp(comp_by_name("curpid")); + hal_parse("tle0.rt_prio = 0.01"); hal_parse("term0.rt_prio = 0.1"); hal_parse("ls0.rt_prio = 0.6"); + hal_parse("pole0.rt_prio = 1.0"); + hal_parse("map0.rt_prio = 2.0"); + hal_parse("rev0.rt_prio = 1.0"); hal_parse("io0.rt_prio = 1.0"); hal_parse("dq0.rt_prio = 2.0"); hal_parse("curpid0.rt_prio = 3.0"); @@ -364,7 +381,7 @@ int main(void) hal_parse("hv0.rt_prio = 6.0"); hal_parse("sim0.rt_prio = 7.0"); - hal_parse("term0.send_step = 100.0"); + hal_parse("term0.send_step = 10.0"); hal_parse("term0.gain0 = 10.0"); hal_parse("term0.gain1 = 10.0"); hal_parse("term0.gain2 = 10.0"); @@ -377,67 +394,50 @@ int main(void) hal_parse("idq0.pos = sim0.vel"); hal_parse("io0.oc1 = idq0.a"); hal_parse("io0.oc2 = idq0.b"); - - //link LS - hal_parse("ls0.mot_temp = io0.mot_temp"); - hal_parse("ls0.dc_volt = io0.udc"); - hal_parse("ls0.hv_temp = io0.hv_temp"); - hal_parse("curpid0.id_cmd = ls0.d_cmd"); - hal_parse("curpid0.iq_cmd = ls0.q_cmd"); - hal_parse("idq0.pos = ls0.pos"); - hal_parse("dq0.pos = ls0.pos"); - hal_parse("hv0.en = ls0.en"); - //ADC TEST - hal_parse("term0.wave3 = io0.udc"); - hal_parse("hv0.udc = io0.udc"); - hal_parse("dq0.u = io0.iu"); - hal_parse("dq0.v = io0.iv"); - hal_parse("dq0.w = io0.iw"); - - // hal_parse("sim0.vel", "idq0.pos"); - // hal_parse("sim0.vel", "dq0.pos"); - - hal_parse("svm0.u = idq0.u"); - hal_parse("svm0.v = idq0.v"); - hal_parse("svm0.w = idq0.w"); - hal_parse("hv0.u = svm0.su"); - - hal_parse("hv0.v = svm0.sv"); - hal_parse("hv0.w = svm0.sw"); - hal_parse("svm0.udc = io0.udc"); - hal_parse("hv0.hv_temp = io0.hv_temp"); hal_parse("curpid0.id_fb = dq0.d"); hal_parse("curpid0.iq_fb = dq0.q"); - hal_parse("ls0.d_fb = dq0.d"); - hal_parse("ls0.q_fb = dq0.q"); - hal_parse("ls0.u_fb = io0.u"); - hal_parse("ls0.v_fb = io0.v"); - hal_parse("ls0.w_fb = io0.w"); hal_parse("idq0.d = curpid0.ud"); hal_parse("idq0.q = curpid0.uq"); - hal_parse("term0.wave0 = curpid0.id_cmd"); - hal_parse("term0.wave1 = curpid0.iq_cmd"); - hal_parse("term0.wave2 = curpid0.id_fb"); - hal_parse("term0.wave3 = curpid0.iq_fb"); - hal_parse("io0.led = term0.con"); - hal_parse("curpid0.rd = ls0.r"); - hal_parse("curpid0.rq = ls0.r"); - hal_parse("curpid0.ld = ls0.l"); - hal_parse("curpid0.lq = ls0.l"); - hal_parse("curpid0.psi = ls0.psi"); - hal_parse("curpid0.kp = ls0.cur_p"); - hal_parse("curpid0.ki = ls0.cur_i"); - hal_parse("curpid0.ff = ls0.cur_ff"); - hal_parse("curpid0.kind = ls0.cur_ind"); - hal_parse("curpid0.max_cur = ls0.max_cur"); - hal_parse("curpid0.pwm_volt = ls0.pwm_volt"); - hal_parse("curpid0.vel = ls0.vel"); + hal_parse("dq0.a = io0.ia"); + hal_parse("dq0.b = io0.ib"); + hal_parse("dq0.pos = sim0.vel"); + + hal_parse("curpid0.rd = 0.5"); + hal_parse("curpid0.rq = 0.5"); + hal_parse("curpid0.ld = 0.001"); + hal_parse("curpid0.lq = 0.001"); + hal_parse("curpid0.psi = 0.005"); + hal_parse("curpid0.kp = 0.1"); + hal_parse("curpid0.ki = 0.0001"); + hal_parse("curpid0.ff = 0"); + hal_parse("curpid0.kind = 0"); + hal_parse("curpid0.max_cur = 5"); + hal_parse("curpid0.pwm_volt = io0.dc_link"); + + // hal_parse("idq0.pos = pole0.cpos"); + hal_parse("pole0.pos = sim0.vel"); + hal_parse("pole0.p = -50.0"); + hal_parse("term0.wave0 = map0.error"); + hal_parse("term0.wave1 = map0.corr"); + hal_parse("map0.pos0 = sim0.vel"); + hal_parse("map0.pos1 = tle0.d3"); + hal_parse("map0.amp = 0.03"); + hal_parse("map0.offset = 2"); + + + // hal parse config // hal_init_nrt(); // error foo hal_start(); + + HAL_TIM_Base_Start(&htim1); + HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); + HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2); + HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_1); + HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_2); while (1) { @@ -494,7 +494,7 @@ void SystemClock_Config(void) PeriphClkInit.Adc12ClockSelection = RCC_ADC12PLLCLK_DIV1; PeriphClkInit.Adc34ClockSelection = RCC_ADC34PLLCLK_DIV1; PeriphClkInit.USBClockSelection = RCC_USBCLKSOURCE_PLL_DIV1_5; - PeriphClkInit.Tim8ClockSelection = RCC_TIM8CLK_PLLCLK; + // PeriphClkInit.Tim8ClockSelection = RCC_TIM8CLK_PLLCLK; PeriphClkInit.Tim1ClockSelection = RCC_TIM1CLK_PLLCLK; PeriphClkInit.RTCClockSelection = RCC_RTCCLKSOURCE_LSI; if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK) diff --git a/stm32f303/src/opamp.c b/stm32f303/src/opamp.c index 0969f62e..7c19fdd2 100644 --- a/stm32f303/src/opamp.c +++ b/stm32f303/src/opamp.c @@ -63,7 +63,7 @@ void MX_OPAMP1_Init(void) hopamp1.Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO0; hopamp1.Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE; hopamp1.Init.PgaConnect = OPAMP_PGA_CONNECT_INVERTINGINPUT_NO; - hopamp1.Init.PgaGain = OPAMP_PGA_GAIN_16; + hopamp1.Init.PgaGain = OPAMP_PGA_GAIN_8; hopamp1.Init.UserTrimming = OPAMP_TRIMMING_FACTORY; if (HAL_OPAMP_Init(&hopamp1) != HAL_OK) { @@ -80,7 +80,7 @@ void MX_OPAMP2_Init(void) hopamp2.Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO0; hopamp2.Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE; hopamp2.Init.PgaConnect = OPAMP_PGA_CONNECT_INVERTINGINPUT_NO; - hopamp2.Init.PgaGain = OPAMP_PGA_GAIN_16; + hopamp2.Init.PgaGain = OPAMP_PGA_GAIN_8; hopamp2.Init.UserTrimming = OPAMP_TRIMMING_FACTORY; if (HAL_OPAMP_Init(&hopamp2) != HAL_OK) { @@ -97,7 +97,7 @@ void MX_OPAMP3_Init(void) hopamp3.Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO0; hopamp3.Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE; hopamp3.Init.PgaConnect = OPAMP_PGA_CONNECT_INVERTINGINPUT_NO; - hopamp3.Init.PgaGain = OPAMP_PGA_GAIN_16; + hopamp3.Init.PgaGain = OPAMP_PGA_GAIN_8; hopamp3.Init.UserTrimming = OPAMP_TRIMMING_FACTORY; if (HAL_OPAMP_Init(&hopamp3) != HAL_OK) { @@ -110,13 +110,13 @@ void MX_OPAMP3_Init(void) void MX_OPAMP4_Init(void) { - hopamp3.Instance = OPAMP4; - hopamp3.Init.Mode = OPAMP_PGA_MODE; - hopamp3.Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO0; - hopamp3.Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE; - hopamp3.Init.PgaConnect = OPAMP_PGA_CONNECT_INVERTINGINPUT_NO; - hopamp3.Init.PgaGain = OPAMP_PGA_GAIN_16; - hopamp3.Init.UserTrimming = OPAMP_TRIMMING_FACTORY; + hopamp4.Instance = OPAMP4; + hopamp4.Init.Mode = OPAMP_PGA_MODE; + hopamp4.Init.NonInvertingInput = OPAMP_NONINVERTINGINPUT_IO3; + hopamp4.Init.TimerControlledMuxmode = OPAMP_TIMERCONTROLLEDMUXMODE_DISABLE; + hopamp4.Init.PgaConnect = OPAMP_PGA_CONNECT_INVERTINGINPUT_NO; + hopamp4.Init.PgaGain = OPAMP_PGA_GAIN_8; + hopamp4.Init.UserTrimming = OPAMP_TRIMMING_FACTORY; if (HAL_OPAMP_Init(&hopamp4) != HAL_OK) { Error_Handler();