Mirrors_Framework/stmbl
1
0
Fork 0
mirror of https://github.com/rene-dev/stmbl.git synced 2026-02-05 09:51:47 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

strom fix, 100k pwm, rl

Browse Source
This commit is contained in:
crinq
2017-10-07 18:21:35 +02:00
parent 8d3d7bbfda
commit ebcecfaa83
9 changed files with 205 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

133
shared/comps/rl.c Normal file
View File

@@ -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),
};

10
shared/f3hw.h
View File

@@ -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

2
shared/hal.c
View File

@@ -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);

3
stm32f303/Makefile
View File

@@ -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

2
stm32f303/STM32F303CBTx_FLASH.ld
View File

@@ -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 */

1
stm32f303/inc/opamp.h
View File

@@ -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 */

38
stm32f303/src/comps/hv.c
View File

@@ -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 = {

42
stm32f303/src/comps/io.c
View File

@@ -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
}

37
stm32f303/src/main.c
View File

@@ -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");