dual test

This commit is contained in:
Rene Hopf 2018-01-06 18:54:51 +01:00
parent b95d11bd57
commit cd72ac6ded
3 changed files with 202 additions and 236 deletions

View File

@ -1,3 +1,23 @@
/*
* This file is part of the stmbl project.
*
* Copyright (C) 2013-2018 Rene Hopf <renehopf@mac.com>
* Copyright (C) 2013-2018 Nico Stute <crinq@crinq.de>
*
* 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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "stm32f1xx_hal.h"

View File

@ -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 <renehopf@mac.com>
* Copyright (C) 2013-2018 Nico Stute <crinq@crinq.de>
*
* 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 <http://www.gnu.org/licenses/>.
*/
/* 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__);

View File

@ -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 <renehopf@mac.com>
* Copyright (C) 2013-2018 Nico Stute <crinq@crinq.de>
*
* 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 <http://www.gnu.org/licenses/>.
*/
/* 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****/