From cd72ac6dedeb6ebbf982046a8af7e0cefa5392fb Mon Sep 17 00:00:00 2001 From: Rene Hopf Date: Sat, 6 Jan 2018 18:54:51 +0100 Subject: [PATCH] dual test --- Inc/defines.h | 20 ++++ Src/gpio.c | 124 +++++++++------------ Src/main.c | 294 +++++++++++++++++++++++--------------------------- 3 files changed, 202 insertions(+), 236 deletions(-) diff --git a/Inc/defines.h b/Inc/defines.h index dbcb5a4..f3df326 100644 --- a/Inc/defines.h +++ b/Inc/defines.h @@ -1,3 +1,23 @@ +/* +* This file is part of the stmbl project. +* +* Copyright (C) 2013-2018 Rene Hopf +* Copyright (C) 2013-2018 Nico Stute +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program. If not, see . +*/ + #pragma once #include "stm32f1xx_hal.h" diff --git a/Src/gpio.c b/Src/gpio.c index 57cf7f3..83c49c6 100644 --- a/Src/gpio.c +++ b/Src/gpio.c @@ -1,43 +1,23 @@ -/** - ****************************************************************************** - * File Name : gpio.c - * Description : This file provides code for the configuration - * of all used GPIO pins. - ****************************************************************************** - ** This notice applies to any and all portions of this file - * that are not between comment pairs USER CODE BEGIN and - * USER CODE END. Other portions of this file, whether - * inserted by the user or by software development tools - * are owned by their respective copyright owners. - * - * COPYRIGHT(c) 2017 STMicroelectronics - * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted provided that the following conditions are met: - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * 3. Neither the name of STMicroelectronics nor the names of its contributors - * may be used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************** - */ +/* +* This file is part of the stmbl project. +* +* Copyright (C) 2013-2018 Rene Hopf +* Copyright (C) 2013-2018 Nico Stute +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program. If not, see . +*/ -/* Includes ------------------------------------------------------------------*/ #include "defines.h" void MX_GPIO_Init(void) @@ -132,23 +112,23 @@ void MX_GPIO_Init(void) GPIO_InitStruct.Pin = LEFT_TIM_WL_PIN; HAL_GPIO_Init(LEFT_TIM_WL_PORT, &GPIO_InitStruct); - // GPIO_InitStruct.Pin = RIGHT_TIM_UH_PIN; - // HAL_GPIO_Init(RIGHT_TIM_UH_PORT, &GPIO_InitStruct); + GPIO_InitStruct.Pin = RIGHT_TIM_UH_PIN; + HAL_GPIO_Init(RIGHT_TIM_UH_PORT, &GPIO_InitStruct); - // GPIO_InitStruct.Pin = RIGHT_TIM_VH_PIN; - // HAL_GPIO_Init(RIGHT_TIM_VH_PORT, &GPIO_InitStruct); + GPIO_InitStruct.Pin = RIGHT_TIM_VH_PIN; + HAL_GPIO_Init(RIGHT_TIM_VH_PORT, &GPIO_InitStruct); - // GPIO_InitStruct.Pin = RIGHT_TIM_WH_PIN; - // HAL_GPIO_Init(RIGHT_TIM_WH_PORT, &GPIO_InitStruct); + GPIO_InitStruct.Pin = RIGHT_TIM_WH_PIN; + HAL_GPIO_Init(RIGHT_TIM_WH_PORT, &GPIO_InitStruct); - // GPIO_InitStruct.Pin = RIGHT_TIM_UL_PIN; - // HAL_GPIO_Init(RIGHT_TIM_UL_PORT, &GPIO_InitStruct); + GPIO_InitStruct.Pin = RIGHT_TIM_UL_PIN; + HAL_GPIO_Init(RIGHT_TIM_UL_PORT, &GPIO_InitStruct); - // GPIO_InitStruct.Pin = RIGHT_TIM_VL_PIN; - // HAL_GPIO_Init(RIGHT_TIM_VL_PORT, &GPIO_InitStruct); + GPIO_InitStruct.Pin = RIGHT_TIM_VL_PIN; + HAL_GPIO_Init(RIGHT_TIM_VL_PORT, &GPIO_InitStruct); - // GPIO_InitStruct.Pin = RIGHT_TIM_WL_PIN; - // HAL_GPIO_Init(RIGHT_TIM_WL_PORT, &GPIO_InitStruct); + GPIO_InitStruct.Pin = RIGHT_TIM_WL_PIN; + HAL_GPIO_Init(RIGHT_TIM_WL_PORT, &GPIO_InitStruct); } TIM_HandleTypeDef htim_right; @@ -216,10 +196,6 @@ void MX_TIM_Init(void){ _Error_Handler(__FILE__, __LINE__); } - - - - htim_left.Instance = LEFT_TIM; htim_left.Init.Prescaler = 0; htim_left.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED1; @@ -284,30 +260,28 @@ void MX_TIM_Init(void){ HAL_TIMEx_PWMN_Start(&htim_left, TIM_CHANNEL_2); HAL_TIMEx_PWMN_Start(&htim_left, TIM_CHANNEL_3); - // HAL_TIM_PWM_Start(&htim_right, TIM_CHANNEL_1); - // HAL_TIM_PWM_Start(&htim_right, TIM_CHANNEL_2); - // HAL_TIM_PWM_Start(&htim_right, TIM_CHANNEL_3); - // HAL_TIMEx_PWMN_Start(&htim_right, TIM_CHANNEL_1); - // HAL_TIMEx_PWMN_Start(&htim_right, TIM_CHANNEL_2); - // HAL_TIMEx_PWMN_Start(&htim_right, TIM_CHANNEL_3); + HAL_TIM_PWM_Start(&htim_right, TIM_CHANNEL_1); + HAL_TIM_PWM_Start(&htim_right, TIM_CHANNEL_2); + HAL_TIM_PWM_Start(&htim_right, TIM_CHANNEL_3); + HAL_TIMEx_PWMN_Start(&htim_right, TIM_CHANNEL_1); + HAL_TIMEx_PWMN_Start(&htim_right, TIM_CHANNEL_2); + HAL_TIMEx_PWMN_Start(&htim_right, TIM_CHANNEL_3); + + htim_left.Instance->RCR = 1; __HAL_TIM_ENABLE(&htim_right); } - - ADC_HandleTypeDef hadc1; ADC_HandleTypeDef hadc2; - -volatile adc_buf_t adc_buffer[1]; +volatile adc_buf_t adc_buffer[3]; void MX_ADC1_Init(void) { ADC_MultiModeTypeDef multimode; ADC_ChannelConfTypeDef sConfig; - __HAL_RCC_ADC1_CLK_ENABLE(); // HAL_ADC_DeInit(&hadc1); @@ -315,12 +289,12 @@ void MX_ADC1_Init(void) /**Common config */ hadc1.Instance = ADC1; - hadc1.Init.ScanConvMode = ADC_SCAN_DISABLE;//ADC_SCAN_ENABLE; + hadc1.Init.ScanConvMode = ADC_SCAN_ENABLE; hadc1.Init.ContinuousConvMode = DISABLE; hadc1.Init.DiscontinuousConvMode = DISABLE; hadc1.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T8_TRGO; hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc1.Init.NbrOfConversion = 1; + hadc1.Init.NbrOfConversion = 3; if (HAL_ADC_Init(&hadc1) != HAL_OK) { _Error_Handler(__FILE__, __LINE__); @@ -333,11 +307,11 @@ void MX_ADC1_Init(void) /**Configure the ADC multi-mode */ - // multimode.Mode = ADC_DUALMODE_REGSIMULT; - // if (HAL_ADCEx_MultiModeConfigChannel(&hadc1, &multimode) != HAL_OK) - // { - // _Error_Handler(__FILE__, __LINE__); - // } + multimode.Mode = ADC_DUALMODE_REGSIMULT; + if (HAL_ADCEx_MultiModeConfigChannel(&hadc1, &multimode) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } /**Configure Regular Channel */ @@ -381,7 +355,7 @@ void MX_ADC1_Init(void) __HAL_RCC_DMA1_CLK_ENABLE(); DMA1_Channel1->CCR = 0; - DMA1_Channel1->CNDTR = 1; + DMA1_Channel1->CNDTR = 3; DMA1_Channel1->CPAR = (uint32_t)&(ADC1->DR); DMA1_Channel1->CMAR = (uint32_t)adc_buffer; DMA1_Channel1->CCR = DMA_CCR_MSIZE_1 | DMA_CCR_PSIZE_1 | DMA_CCR_MINC | DMA_CCR_CIRC | DMA_CCR_TCIE; @@ -408,7 +382,7 @@ void MX_ADC2_Init(void) hadc2.Init.DiscontinuousConvMode = DISABLE; hadc2.Init.ExternalTrigConv = ADC_SOFTWARE_START; hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT; - hadc2.Init.NbrOfConversion = 1; + hadc2.Init.NbrOfConversion = 3; if (HAL_ADC_Init(&hadc2) != HAL_OK) { _Error_Handler(__FILE__, __LINE__); diff --git a/Src/main.c b/Src/main.c index 0676cf9..32696d3 100644 --- a/Src/main.c +++ b/Src/main.c @@ -1,82 +1,44 @@ -/** - ****************************************************************************** - * File Name : main.c - * Description : Main program body - ****************************************************************************** - ** This notice applies to any and all portions of this file - * that are not between comment pairs USER CODE BEGIN and - * USER CODE END. Other portions of this file, whether - * inserted by the user or by software development tools - * are owned by their respective copyright owners. - * - * COPYRIGHT(c) 2017 STMicroelectronics - * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted provided that the following conditions are met: - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * 3. Neither the name of STMicroelectronics nor the names of its contributors - * may be used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************** - */ +/* +* This file is part of the stmbl project. +* +* Copyright (C) 2013-2018 Rene Hopf +* Copyright (C) 2013-2018 Nico Stute +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program. If not, see . +*/ -/* Includes ------------------------------------------------------------------*/ #include "main.h" #include "stm32f1xx_hal.h" #include "defines.h" #include "gpio.h" - -/* USER CODE BEGIN Includes */ - -/* USER CODE END Includes */ - -/* Private variables ---------------------------------------------------------*/ - -/* USER CODE BEGIN PV */ -/* Private variables ---------------------------------------------------------*/ - -/* USER CODE END PV */ - -/* Private function prototypes -----------------------------------------------*/ void SystemClock_Config(void); -/* USER CODE BEGIN PFP */ -/* Private function prototypes -----------------------------------------------*/ - -/* USER CODE END PFP */ - -/* USER CODE BEGIN 0 */ - -/* USER CODE END 0 */ - extern TIM_HandleTypeDef htim_left; extern TIM_HandleTypeDef htim_right; extern ADC_HandleTypeDef hadc1; extern ADC_HandleTypeDef hadc2; -extern volatile adc_buf_t adc_buffer[1]; +extern volatile adc_buf_t adc_buffer[3]; + +volatile int posl = 0; +volatile int posr = 0; +volatile int pwml = 0; +volatile int pwmr = 0; -volatile int pos = 0; const int pwm_res = 64000000 / 2 / PWM_FREQ; -volatile int pwm = 0; -uint8_t hall_to_pos[8] = { +const uint8_t hall_to_pos[8] = { 0, 0, 2, @@ -87,6 +49,45 @@ uint8_t hall_to_pos[8] = { 0, }; +inline void block(int pwm, int pos, int* u, int* v, int* w){ + switch(pos){ + case 0: + *u = 0; + *v = pwm; + *w = -pwm; + break; + case 1: + *u = -pwm; + *v = pwm; + *w = 0; + break; + case 2: + *u = -pwm; + *v = 0; + *w = pwm; + break; + case 3: + *u = 0; + *v = -pwm; + *w = pwm; + break; + case 4: + *u = pwm; + *v = -pwm; + *w = 0; + break; + case 5: + *u = pwm; + *v = 0; + *w = -pwm; + break; + default: + *u = 0; + *v = 0; + *w = 0; + } +} + int last_pos = 0; int timer = 0; int max_time = PWM_FREQ / 10; @@ -94,85 +95,65 @@ volatile int vel = 0; void DMA1_Channel1_IRQHandler(){ DMA1->IFCR = DMA_IFCR_CTCIF1; - int u = 0; - int v = 0; - int w = 0; + HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1); - uint8_t hall_u = HAL_GPIO_ReadPin(LEFT_HALL_U_PORT, LEFT_HALL_U_PIN); - uint8_t hall_v = HAL_GPIO_ReadPin(LEFT_HALL_V_PORT, LEFT_HALL_V_PIN); - uint8_t hall_w = HAL_GPIO_ReadPin(LEFT_HALL_W_PORT, LEFT_HALL_W_PIN); + int ul = 0; + int vl = 0; + int wl = 0; - uint8_t hall = hall_u * 1 + hall_v * 2 + hall_w * 4; - pos = hall_to_pos[hall]; - pos += 2; - pos %= 6; + int ur = 0; + int vr = 0; + int wr = 0; + + uint8_t hall_ul = HAL_GPIO_ReadPin(LEFT_HALL_U_PORT, LEFT_HALL_U_PIN); + uint8_t hall_vl = HAL_GPIO_ReadPin(LEFT_HALL_V_PORT, LEFT_HALL_V_PIN); + uint8_t hall_wl = HAL_GPIO_ReadPin(LEFT_HALL_W_PORT, LEFT_HALL_W_PIN); + + uint8_t hall_ur = HAL_GPIO_ReadPin(RIGHT_HALL_U_PORT, RIGHT_HALL_U_PIN); + uint8_t hall_vr = HAL_GPIO_ReadPin(RIGHT_HALL_V_PORT, RIGHT_HALL_V_PIN); + uint8_t hall_wr = HAL_GPIO_ReadPin(RIGHT_HALL_W_PORT, RIGHT_HALL_W_PIN); + + uint8_t halll = hall_ul * 1 + hall_vl * 2 + hall_wl * 4; + posl = hall_to_pos[halll]; + posl += 2; + posl %= 6; + + uint8_t hallr = hall_ur * 1 + hall_vr * 2 + hall_wr * 4; + posr = hall_to_pos[hallr]; + posr += 2; + posr %= 6; timer++; - if(timer > max_time){ - timer = max_time; - vel = 0; - } - - if(pos != last_pos){ - vel = 1000 * PWM_FREQ / timer / P / 6 * 2; - - if((pos - last_pos + 6) % 6 > 2){ - vel = -vel; - } - - timer = 0; - } - last_pos = pos; - - switch(pos){ - case 0: - u = 0; - v = pwm; - w = -pwm; - break; - case 1: - u = -pwm; - v = pwm; - w = 0; - break; - case 2: - u = -pwm; - v = 0; - w = pwm; - break; - case 3: - u = 0; - v = -pwm; - w = pwm; - break; - case 4: - u = pwm; - v = -pwm; - w = 0; - break; - case 5: - u = pwm; - v = 0; - w = -pwm; - break; - default: - u = 0; - v = 0; - w = 0; - } - - LEFT_TIM->LEFT_TIM_U = CLAMP(u + pwm_res / 2, 0, pwm_res); - LEFT_TIM->LEFT_TIM_V = CLAMP(v + pwm_res / 2, 0, pwm_res); - LEFT_TIM->LEFT_TIM_W = CLAMP(w + pwm_res / 2, 0, pwm_res); - // while(!(ADC1->SR & 2)){ - + // if(timer > max_time){ + // timer = max_time; + // vel = 0; // } - // while(!(DMA1->ISR & 2)){} - // DMA1->IFCR = 1; - - // } + // if(pos != last_pos){ + // vel = 1000 * PWM_FREQ / timer / P / 6 * 2; + + // if((pos - last_pos + 6) % 6 > 2){ + // vel = -vel; + // } + + // timer = 0; + // } + // last_pos = pos; + + 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); + + 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); } int milli_vel_error_sum = 0; @@ -200,23 +181,24 @@ int main(void) while (1) { HAL_Delay(0); - int milli_cur = 3000; - int milli_volt = milli_cur * MILLI_R / 1000;// + vel * MILLI_PSI * 141; - // pwm = milli_volt * pwm_res / MILLI_V; + // int milli_cur = 3000; + // int milli_volt = milli_cur * MILLI_R / 1000;// + vel * MILLI_PSI * 141; + // // pwm = milli_volt * pwm_res / MILLI_V; - int milli_vel_cmd = 200; - int milli_vel_error = milli_vel_cmd - vel; - 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); + // int milli_vel_cmd = 200; + // int milli_vel_error = milli_vel_cmd - vel; + // 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); + pwml = 100; + pwmr = 100; - - if(vel > milli_vel_cmd){ - HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1); - } - else{ - HAL_GPIO_WritePin(LED_PORT, LED_PIN, 0); - } + // if(vel > milli_vel_cmd){ + // HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1); + // } + // else{ + // HAL_GPIO_WritePin(LED_PORT, LED_PIN, 0); + // } } } @@ -313,13 +295,3 @@ void assert_failed(uint8_t* file, uint32_t line) } #endif - -/** - * @} - */ - -/** - * @} -*/ - -/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/