interfisch
/
hoverboard-firmware-hack-serial-esc
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
Inc/defines.h
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 #pragma once
#include "stm32f1xx_hal.h" #include "stm32f1xx_hal.h"

124
Src/gpio.c
View File

@ -1,43 +1,23 @@
/** /*
****************************************************************************** * This file is part of the stmbl project.
* File Name : gpio.c *
* Description : This file provides code for the configuration * Copyright (C) 2013-2018 Rene Hopf <renehopf@mac.com>
* of all used GPIO pins. * Copyright (C) 2013-2018 Nico Stute <crinq@crinq.de>
****************************************************************************** *
** This notice applies to any and all portions of this file * This program is free software: you can redistribute it and/or modify
* that are not between comment pairs USER CODE BEGIN and * it under the terms of the GNU General Public License as published by
* USER CODE END. Other portions of this file, whether * the Free Software Foundation, either version 3 of the License, or
* inserted by the user or by software development tools * (at your option) any later version.
* are owned by their respective copyright owners. *
* * This program is distributed in the hope that it will be useful,
* COPYRIGHT(c) 2017 STMicroelectronics * but WITHOUT ANY WARRANTY; without even the implied warranty of
* * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* Redistribution and use in source and binary forms, with or without modification, * GNU General Public License for more details.
* are permitted provided that the following conditions are met: *
* 1. Redistributions of source code must retain the above copyright notice, * You should have received a copy of the GNU General Public License
* this list of conditions and the following disclaimer. * along with this program. If not, see <http://www.gnu.org/licenses/>.
* 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.
*
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "defines.h" #include "defines.h"
void MX_GPIO_Init(void) void MX_GPIO_Init(void)
@ -132,23 +112,23 @@ void MX_GPIO_Init(void)
GPIO_InitStruct.Pin = LEFT_TIM_WL_PIN; GPIO_InitStruct.Pin = LEFT_TIM_WL_PIN;
HAL_GPIO_Init(LEFT_TIM_WL_PORT, &GPIO_InitStruct); HAL_GPIO_Init(LEFT_TIM_WL_PORT, &GPIO_InitStruct);
// GPIO_InitStruct.Pin = RIGHT_TIM_UH_PIN; GPIO_InitStruct.Pin = RIGHT_TIM_UH_PIN;
// HAL_GPIO_Init(RIGHT_TIM_UH_PORT, &GPIO_InitStruct); HAL_GPIO_Init(RIGHT_TIM_UH_PORT, &GPIO_InitStruct);
// GPIO_InitStruct.Pin = RIGHT_TIM_VH_PIN; GPIO_InitStruct.Pin = RIGHT_TIM_VH_PIN;
// HAL_GPIO_Init(RIGHT_TIM_VH_PORT, &GPIO_InitStruct); HAL_GPIO_Init(RIGHT_TIM_VH_PORT, &GPIO_InitStruct);
// GPIO_InitStruct.Pin = RIGHT_TIM_WH_PIN; GPIO_InitStruct.Pin = RIGHT_TIM_WH_PIN;
// HAL_GPIO_Init(RIGHT_TIM_WH_PORT, &GPIO_InitStruct); HAL_GPIO_Init(RIGHT_TIM_WH_PORT, &GPIO_InitStruct);
// GPIO_InitStruct.Pin = RIGHT_TIM_UL_PIN; GPIO_InitStruct.Pin = RIGHT_TIM_UL_PIN;
// HAL_GPIO_Init(RIGHT_TIM_UL_PORT, &GPIO_InitStruct); HAL_GPIO_Init(RIGHT_TIM_UL_PORT, &GPIO_InitStruct);
// GPIO_InitStruct.Pin = RIGHT_TIM_VL_PIN; GPIO_InitStruct.Pin = RIGHT_TIM_VL_PIN;
// HAL_GPIO_Init(RIGHT_TIM_VL_PORT, &GPIO_InitStruct); HAL_GPIO_Init(RIGHT_TIM_VL_PORT, &GPIO_InitStruct);
// GPIO_InitStruct.Pin = RIGHT_TIM_WL_PIN; GPIO_InitStruct.Pin = RIGHT_TIM_WL_PIN;
// HAL_GPIO_Init(RIGHT_TIM_WL_PORT, &GPIO_InitStruct); HAL_GPIO_Init(RIGHT_TIM_WL_PORT, &GPIO_InitStruct);
} }
TIM_HandleTypeDef htim_right; TIM_HandleTypeDef htim_right;
@ -216,10 +196,6 @@ void MX_TIM_Init(void){
_Error_Handler(__FILE__, __LINE__); _Error_Handler(__FILE__, __LINE__);
} }
htim_left.Instance = LEFT_TIM; htim_left.Instance = LEFT_TIM;
htim_left.Init.Prescaler = 0; htim_left.Init.Prescaler = 0;
htim_left.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED1; 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_2);
HAL_TIMEx_PWMN_Start(&htim_left, TIM_CHANNEL_3); 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_1);
// HAL_TIM_PWM_Start(&htim_right, TIM_CHANNEL_2); HAL_TIM_PWM_Start(&htim_right, TIM_CHANNEL_2);
// HAL_TIM_PWM_Start(&htim_right, TIM_CHANNEL_3); 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_1);
// HAL_TIMEx_PWMN_Start(&htim_right, TIM_CHANNEL_2); HAL_TIMEx_PWMN_Start(&htim_right, TIM_CHANNEL_2);
// HAL_TIMEx_PWMN_Start(&htim_right, TIM_CHANNEL_3); HAL_TIMEx_PWMN_Start(&htim_right, TIM_CHANNEL_3);
htim_left.Instance->RCR = 1;
__HAL_TIM_ENABLE(&htim_right); __HAL_TIM_ENABLE(&htim_right);
} }
ADC_HandleTypeDef hadc1; ADC_HandleTypeDef hadc1;
ADC_HandleTypeDef hadc2; ADC_HandleTypeDef hadc2;
volatile adc_buf_t adc_buffer[3];
volatile adc_buf_t adc_buffer[1];
void MX_ADC1_Init(void) void MX_ADC1_Init(void)
{ {
ADC_MultiModeTypeDef multimode; ADC_MultiModeTypeDef multimode;
ADC_ChannelConfTypeDef sConfig; ADC_ChannelConfTypeDef sConfig;
__HAL_RCC_ADC1_CLK_ENABLE(); __HAL_RCC_ADC1_CLK_ENABLE();
// HAL_ADC_DeInit(&hadc1); // HAL_ADC_DeInit(&hadc1);
@ -315,12 +289,12 @@ void MX_ADC1_Init(void)
/**Common config /**Common config
*/ */
hadc1.Instance = ADC1; hadc1.Instance = ADC1;
hadc1.Init.ScanConvMode = ADC_SCAN_DISABLE;//ADC_SCAN_ENABLE; hadc1.Init.ScanConvMode = ADC_SCAN_ENABLE;
hadc1.Init.ContinuousConvMode = DISABLE; hadc1.Init.ContinuousConvMode = DISABLE;
hadc1.Init.DiscontinuousConvMode = DISABLE; hadc1.Init.DiscontinuousConvMode = DISABLE;
hadc1.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T8_TRGO; hadc1.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T8_TRGO;
hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT; hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc1.Init.NbrOfConversion = 1; hadc1.Init.NbrOfConversion = 3;
if (HAL_ADC_Init(&hadc1) != HAL_OK) if (HAL_ADC_Init(&hadc1) != HAL_OK)
{ {
_Error_Handler(__FILE__, __LINE__); _Error_Handler(__FILE__, __LINE__);
@ -333,11 +307,11 @@ void MX_ADC1_Init(void)
/**Configure the ADC multi-mode /**Configure the ADC multi-mode
*/ */
// multimode.Mode = ADC_DUALMODE_REGSIMULT; multimode.Mode = ADC_DUALMODE_REGSIMULT;
// if (HAL_ADCEx_MultiModeConfigChannel(&hadc1, &multimode) != HAL_OK) if (HAL_ADCEx_MultiModeConfigChannel(&hadc1, &multimode) != HAL_OK)
// { {
// _Error_Handler(__FILE__, __LINE__); _Error_Handler(__FILE__, __LINE__);
// } }
/**Configure Regular Channel /**Configure Regular Channel
*/ */
@ -381,7 +355,7 @@ void MX_ADC1_Init(void)
__HAL_RCC_DMA1_CLK_ENABLE(); __HAL_RCC_DMA1_CLK_ENABLE();
DMA1_Channel1->CCR = 0; DMA1_Channel1->CCR = 0;
DMA1_Channel1->CNDTR = 1; DMA1_Channel1->CNDTR = 3;
DMA1_Channel1->CPAR = (uint32_t)&(ADC1->DR); DMA1_Channel1->CPAR = (uint32_t)&(ADC1->DR);
DMA1_Channel1->CMAR = (uint32_t)adc_buffer; 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; 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.DiscontinuousConvMode = DISABLE;
hadc2.Init.ExternalTrigConv = ADC_SOFTWARE_START; hadc2.Init.ExternalTrigConv = ADC_SOFTWARE_START;
hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT; hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc2.Init.NbrOfConversion = 1; hadc2.Init.NbrOfConversion = 3;
if (HAL_ADC_Init(&hadc2) != HAL_OK) if (HAL_ADC_Init(&hadc2) != HAL_OK)
{ {
_Error_Handler(__FILE__, __LINE__); _Error_Handler(__FILE__, __LINE__);

292
Src/main.c
View File

@ -1,82 +1,44 @@
/** /*
****************************************************************************** * This file is part of the stmbl project.
* File Name : main.c *
* Description : Main program body * Copyright (C) 2013-2018 Rene Hopf <renehopf@mac.com>
****************************************************************************** * Copyright (C) 2013-2018 Nico Stute <crinq@crinq.de>
** This notice applies to any and all portions of this file *
* that are not between comment pairs USER CODE BEGIN and * This program is free software: you can redistribute it and/or modify
* USER CODE END. Other portions of this file, whether * it under the terms of the GNU General Public License as published by
* inserted by the user or by software development tools * the Free Software Foundation, either version 3 of the License, or
* are owned by their respective copyright owners. * (at your option) any later version.
* *
* COPYRIGHT(c) 2017 STMicroelectronics * This program is distributed in the hope that it will be useful,
* * but WITHOUT ANY WARRANTY; without even the implied warranty of
* Redistribution and use in source and binary forms, with or without modification, * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* are permitted provided that the following conditions are met: * GNU General Public License for more details.
* 1. Redistributions of source code must retain the above copyright notice, *
* this list of conditions and the following disclaimer. * You should have received a copy of the GNU General Public License
* 2. Redistributions in binary form must reproduce the above copyright notice, * along with this program. If not, see <http://www.gnu.org/licenses/>.
* 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.
*
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "main.h" #include "main.h"
#include "stm32f1xx_hal.h" #include "stm32f1xx_hal.h"
#include "defines.h" #include "defines.h"
#include "gpio.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); 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_left;
extern TIM_HandleTypeDef htim_right; extern TIM_HandleTypeDef htim_right;
extern ADC_HandleTypeDef hadc1; extern ADC_HandleTypeDef hadc1;
extern ADC_HandleTypeDef hadc2; 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; const int pwm_res = 64000000 / 2 / PWM_FREQ;
volatile int pwm = 0; const uint8_t hall_to_pos[8] = {
uint8_t hall_to_pos[8] = {
0, 0,
0, 0,
2, 2,
@ -87,6 +49,45 @@ uint8_t hall_to_pos[8] = {
0, 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 last_pos = 0;
int timer = 0; int timer = 0;
int max_time = PWM_FREQ / 10; int max_time = PWM_FREQ / 10;
@ -94,85 +95,65 @@ volatile int vel = 0;
void DMA1_Channel1_IRQHandler(){ void DMA1_Channel1_IRQHandler(){
DMA1->IFCR = DMA_IFCR_CTCIF1; DMA1->IFCR = DMA_IFCR_CTCIF1;
int u = 0; HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
int v = 0;
int w = 0;
uint8_t hall_u = HAL_GPIO_ReadPin(LEFT_HALL_U_PORT, LEFT_HALL_U_PIN); int ul = 0;
uint8_t hall_v = HAL_GPIO_ReadPin(LEFT_HALL_V_PORT, LEFT_HALL_V_PIN); int vl = 0;
uint8_t hall_w = HAL_GPIO_ReadPin(LEFT_HALL_W_PORT, LEFT_HALL_W_PIN); int wl = 0;
uint8_t hall = hall_u * 1 + hall_v * 2 + hall_w * 4; int ur = 0;
pos = hall_to_pos[hall]; int vr = 0;
pos += 2; int wr = 0;
pos %= 6;
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++; timer++;
if(timer > max_time){ // if(timer > max_time){
timer = max_time; // timer = max_time;
vel = 0; // 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)){
// } // }
// 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; int milli_vel_error_sum = 0;
@ -200,23 +181,24 @@ int main(void)
while (1) while (1)
{ {
HAL_Delay(0); HAL_Delay(0);
int milli_cur = 3000; // int milli_cur = 3000;
int milli_volt = milli_cur * MILLI_R / 1000;// + vel * MILLI_PSI * 141; // int milli_volt = milli_cur * MILLI_R / 1000;// + vel * MILLI_PSI * 141;
// pwm = milli_volt * pwm_res / MILLI_V; // // pwm = milli_volt * pwm_res / MILLI_V;
int milli_vel_cmd = 200; // int milli_vel_cmd = 200;
int milli_vel_error = milli_vel_cmd - vel; // int milli_vel_error = milli_vel_cmd - vel;
milli_vel_error_sum += milli_vel_error; // milli_vel_error_sum += milli_vel_error;
milli_vel_error_sum = CLAMP(milli_vel_error_sum, -200000, 200000); // milli_vel_error_sum = CLAMP(milli_vel_error_sum, -200000, 200000);
pwm = CLAMP(milli_vel_cmd / 5 + milli_vel_error_sum / 200, -500, 500); // pwm = CLAMP(milli_vel_cmd / 5 + milli_vel_error_sum / 200, -500, 500);
pwml = 100;
pwmr = 100;
// if(vel > milli_vel_cmd){
if(vel > milli_vel_cmd){ // HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1); // }
} // else{
else{ // HAL_GPIO_WritePin(LED_PORT, LED_PIN, 0);
HAL_GPIO_WritePin(LED_PORT, LED_PIN, 0); // }
}
} }
} }
@ -313,13 +295,3 @@ void assert_failed(uint8_t* file, uint32_t line)
} }
#endif #endif
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/