From 86c6106fb76e4d965b44d4a3f82a3e0de5273a64 Mon Sep 17 00:00:00 2001 From: Rene Hopf Date: Sun, 7 Jan 2018 03:11:55 +0100 Subject: [PATCH] current feedback "working" --- Inc/defines.h | 6 +-- Src/main.c | 139 ++++++++++++++++++++++++++++++++++++++------------ Src/setup.c | 45 ++++++++++++++-- 3 files changed, 150 insertions(+), 40 deletions(-) diff --git a/Inc/defines.h b/Inc/defines.h index 9a52200..e63d6c2 100644 --- a/Inc/defines.h +++ b/Inc/defines.h @@ -118,7 +118,7 @@ #define CHARGER_PORT GPIOA #define PWM_FREQ 16000 -#define DEAD_TIME 50 +#define DEAD_TIME 32 #define R 0.27 #define P 15 @@ -151,8 +151,8 @@ typedef struct { uint16_t rr2; uint16_t rl1; uint16_t rl2; - uint16_t r_dc1; - uint16_t l_dc2; + uint16_t dcr; + uint16_t dcl; uint16_t batt1; uint16_t l_tx2; uint16_t bat1; diff --git a/Src/main.c b/Src/main.c index 203de1e..9b4da6a 100644 --- a/Src/main.c +++ b/Src/main.c @@ -87,55 +87,102 @@ inline void block(int pwm, int pos, int *u, int *v, int *w) { } } +inline void block2(int pos, int u, int v, int *q) { + switch(pos) { + case 0: + *q = u - v; + // *u = 0; + // *v = pwm; + // *w = -pwm; + break; + case 1: + *q = u; + // *u = -pwm; + // *v = pwm; + // *w = 0; + break; + case 2: + *q = u; + // *u = -pwm; + // *v = 0; + // *w = pwm; + break; + case 3: + *q = v; + // *u = 0; + // *v = -pwm; + // *w = pwm; + break; + case 4: + *q = v; + // *u = pwm; + // *v = -pwm; + // *w = 0; + break; + case 5: + *q = -(u - v); + // *u = pwm; + // *v = 0; + // *w = -pwm; + break; + default: + *q = 0; + // *u = 0; + // *v = 0; + // *w = 0; + } +} + int last_pos = 0; int timer = 0; int max_time = PWM_FREQ / 10; volatile int vel = 0; +int offsetcount = 0; +int offsetrl1 = 2000; +int offsetrl2 = 2000; +int offsetrr1 = 2000; +int offsetrr2 = 2000; +int offsetdcl = 2000; +int offsetdcr = 2000; + volatile uint8_t uart_buf[10]; +int curl = 0; +// int errorl = 0; +// int kp = 5; +// volatile int cmdl = 0; void DMA1_Channel1_IRQHandler() { DMA1->IFCR = DMA_IFCR_CTCIF1; - HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1); - /* - uart_buf[0] = 0xff; - uart_buf[1] = adc_buffer.r_dc1 - 1850 + 127; - uart_buf[2] = adc_buffer.l_dc2 - 1850 + 127; - uart_buf[3] = 127;//adc_buffer.rr1 - 2000 + 127; - uart_buf[4] = 127;//adc_buffer.rr2 - 2000 + 127; - uart_buf[5] = 127;//adc_buffer.rl1 - 2000 + 127; - uart_buf[6] = 127;//adc_buffer.rl2 - 2000 + 127; - uart_buf[7] = adc_buffer.batt1 - 1550 + 127; - uart_buf[8] = adc_buffer.bat1 - 1550 + 127; - uart_buf[9] = '\n'; + // HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1); - if(DMA1_Channel2->CNDTR == 0){ - DMA1_Channel2->CCR &= ~DMA_CCR_EN; - DMA1_Channel2->CNDTR = 10; - DMA1_Channel2->CMAR = (uint32_t)uart_buf; - DMA1_Channel2->CCR |= DMA_CCR_EN; + if(offsetcount < 1000) { + offsetcount++; + offsetrl1 = (adc_buffer.rl1 + offsetrl1) / 2; + offsetrl2 = (adc_buffer.rl2 + offsetrl2) / 2; + offsetrr1 = (adc_buffer.rr1 + offsetrr1) / 2; + offsetrr2 = (adc_buffer.rr2 + offsetrr2) / 2; + offsetdcl = (adc_buffer.dcl + offsetdcl) / 2; + offsetdcr = (adc_buffer.dcr + offsetdcr) / 2; + return; } - */ - if(adc_buffer.l_dc2 > 1950) { + if(adc_buffer.dcl - offsetdcl > 40) { LEFT_TIM->BDTR &= ~TIM_BDTR_MOE; + HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1); } else { LEFT_TIM->BDTR |= TIM_BDTR_MOE; + HAL_GPIO_WritePin(LED_PORT, LED_PIN, 0); } - if(adc_buffer.r_dc1 > 1950) { + if(adc_buffer.dcr - offsetdcr > 40) { RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE; } else { RIGHT_TIM->BDTR |= TIM_BDTR_MOE; } - int ul = 0; - int vl = 0; - int wl = 0; - - int ur = 0; - int vr = 0; - int wr = 0; + int ul, vl, wl; + int ur, vr, wr; uint8_t hall_ul = !(LEFT_HALL_U_PORT->IDR & LEFT_HALL_U_PIN); uint8_t hall_vl = !(LEFT_HALL_V_PORT->IDR & LEFT_HALL_V_PIN); @@ -155,6 +202,25 @@ void DMA1_Channel1_IRQHandler() { posr += 2; posr %= 6; + block2(posl, adc_buffer.rl1 - offsetrl1, adc_buffer.rl2 - offsetrl2, &curl); + uart_buf[0] = 0xff; + uart_buf[1] = 127; //adc_buffer.dcl - 1850 + 127; + uart_buf[2] = 127; //adc_buffer.dcr - 1850 + 127; + uart_buf[3] = 127; ////CLAMP((adc_buffer.rr1 - offsetrr1) / 8 + 127,0,255); + uart_buf[4] = 127; ////CLAMP((adc_buffer.rr2 - offsetrr2) / 8 + 127,0,255); + uart_buf[5] = CLAMP((adc_buffer.rl1 - offsetrl1) / 8 + 127, 0, 255); + uart_buf[6] = CLAMP((adc_buffer.rl2 - offsetrl2) / 8 + 127, 0, 255); + uart_buf[7] = 127; //CLAMP(curl / 8 + 127,0,255);//adc_buffer.batt1 - 1550 + 127; + uart_buf[8] = 127 + posl * 20; //adc_buffer.bat1 - 1550 + 127; + uart_buf[9] = '\n'; + + if(DMA1_Channel2->CNDTR == 0) { + DMA1_Channel2->CCR &= ~DMA_CCR_EN; + DMA1_Channel2->CNDTR = 10; + DMA1_Channel2->CMAR = (uint32_t)uart_buf; + DMA1_Channel2->CCR |= DMA_CCR_EN; + } + timer++; // if(timer > max_time){ @@ -173,17 +239,21 @@ void DMA1_Channel1_IRQHandler() { // } // last_pos = pos; + //YOLOTEST + // errorl = cmdl - curl; + // pwml = kp * errorl; + block(pwml, posl, &ul, &vl, &wl); block(pwmr, posr, &ur, &vr, &wr); - LEFT_TIM->LEFT_TIM_U = CLAMP(ul + pwm_res / 2, 0, pwm_res); - LEFT_TIM->LEFT_TIM_V = CLAMP(vl + pwm_res / 2, 0, pwm_res); - LEFT_TIM->LEFT_TIM_W = CLAMP(wl + pwm_res / 2, 0, pwm_res); + LEFT_TIM->LEFT_TIM_U = CLAMP(ul + pwm_res / 2, 10, pwm_res-10); + LEFT_TIM->LEFT_TIM_V = CLAMP(vl + pwm_res / 2, 10, pwm_res-10); + LEFT_TIM->LEFT_TIM_W = CLAMP(wl + pwm_res / 2, 10, pwm_res-10); - RIGHT_TIM->RIGHT_TIM_U = CLAMP(ur + pwm_res / 2, 0, pwm_res); - RIGHT_TIM->RIGHT_TIM_V = CLAMP(vr + pwm_res / 2, 0, pwm_res); - RIGHT_TIM->RIGHT_TIM_W = CLAMP(wr + pwm_res / 2, 0, pwm_res); - HAL_GPIO_WritePin(LED_PORT, LED_PIN, 0); + RIGHT_TIM->RIGHT_TIM_U = CLAMP(ur + pwm_res / 2, 10, pwm_res-10); + RIGHT_TIM->RIGHT_TIM_V = CLAMP(vr + pwm_res / 2, 10, pwm_res-10); + RIGHT_TIM->RIGHT_TIM_W = CLAMP(wr + pwm_res / 2, 10, pwm_res-10); + // HAL_GPIO_WritePin(LED_PORT, LED_PIN, 0); } int milli_vel_error_sum = 0; @@ -233,6 +303,7 @@ int main(void) { // milli_vel_error_sum += milli_vel_error; // milli_vel_error_sum = CLAMP(milli_vel_error_sum, -200000, 200000); // pwm = CLAMP(milli_vel_cmd / 5 + milli_vel_error_sum / 200, -500, 500); + // cmdl = 70; pwml = 150; pwmr = 150; diff --git a/Src/setup.c b/Src/setup.c index e4528ab..47dd73f 100644 --- a/Src/setup.c +++ b/Src/setup.c @@ -73,6 +73,38 @@ void UART_Init() { DMA1->IFCR = DMA_IFCR_CTCIF2 | DMA_IFCR_CHTIF2 | DMA_IFCR_CGIF2; } +/* +void UART_Init() { + __HAL_RCC_USART2_CLK_ENABLE(); + __HAL_RCC_DMA1_CLK_ENABLE(); + + UART_HandleTypeDef huart2; + huart2.Instance = USART2; + huart2.Init.BaudRate = 115200; + huart2.Init.WordLength = UART_WORDLENGTH_8B; + huart2.Init.StopBits = UART_STOPBITS_1; + huart2.Init.Parity = UART_PARITY_NONE; + huart2.Init.Mode = UART_MODE_TX; + huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE; + huart2.Init.OverSampling = UART_OVERSAMPLING_16; + HAL_UART_Init(&huart2); + + USART2->CR3 |= USART_CR3_DMAT; // | USART_CR3_DMAR | USART_CR3_OVRDIS; + + GPIO_InitTypeDef GPIO_InitStruct; + GPIO_InitStruct.Pin = GPIO_PIN_2; + GPIO_InitStruct.Pull = GPIO_PULLUP; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + DMA1_Channel7->CCR = 0; + DMA1_Channel7->CPAR = (uint32_t) & (USART3->DR); + DMA1_Channel7->CNDTR = 0; + DMA1_Channel7->CCR = DMA_CCR_MINC | DMA_CCR_DIR; + DMA1->IFCR = DMA_IFCR_CTCIF7 | DMA_IFCR_CHTIF7 | DMA_IFCR_CGIF7; +} +*/ void MX_GPIO_Init(void) { GPIO_InitTypeDef GPIO_InitStruct; @@ -261,6 +293,9 @@ void MX_TIM_Init(void) { sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; HAL_TIMEx_ConfigBreakDeadTime(&htim_left, &sBreakDeadTimeConfig); + LEFT_TIM->BDTR &= ~TIM_BDTR_MOE; + RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE; + HAL_TIM_PWM_Start(&htim_left, TIM_CHANNEL_1); HAL_TIM_PWM_Start(&htim_left, TIM_CHANNEL_2); HAL_TIM_PWM_Start(&htim_left, TIM_CHANNEL_3); @@ -304,16 +339,18 @@ void MX_ADC1_Init(void) { multimode.Mode = ADC_DUALMODE_REGSIMULT; HAL_ADCEx_MultiModeConfigChannel(&hadc1, &multimode); - sConfig.SamplingTime = ADC_SAMPLETIME_13CYCLES_5; + sConfig.SamplingTime = ADC_SAMPLETIME_7CYCLES_5; sConfig.Channel = ADC_CHANNEL_14; sConfig.Rank = 1; HAL_ADC_ConfigChannel(&hadc1, &sConfig); - sConfig.Channel = ADC_CHANNEL_1; + sConfig.Channel = ADC_CHANNEL_0; sConfig.Rank = 2; HAL_ADC_ConfigChannel(&hadc1, &sConfig); + sConfig.SamplingTime = ADC_SAMPLETIME_13CYCLES_5; + sConfig.Channel = ADC_CHANNEL_11; sConfig.Rank = 3; HAL_ADC_ConfigChannel(&hadc1, &sConfig); @@ -362,7 +399,7 @@ void MX_ADC2_Init(void) { hadc2.Init.NbrOfConversion = 5; HAL_ADC_Init(&hadc2); - sConfig.SamplingTime = ADC_SAMPLETIME_13CYCLES_5; + sConfig.SamplingTime = ADC_SAMPLETIME_7CYCLES_5; sConfig.Channel = ADC_CHANNEL_15; sConfig.Rank = 1; @@ -372,6 +409,8 @@ void MX_ADC2_Init(void) { sConfig.Rank = 2; HAL_ADC_ConfigChannel(&hadc2, &sConfig); + sConfig.SamplingTime = ADC_SAMPLETIME_13CYCLES_5; + sConfig.Channel = ADC_CHANNEL_10; sConfig.Rank = 3; HAL_ADC_ConfigChannel(&hadc2, &sConfig);