POSITION

/* USER CODE BEGIN Header */

/**

******************************************************************************

* @file : main.c

* @brief : 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) 2019 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.

******************************************************************************

*/

/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "main.h"

/* Private includes ----------------------------------------------------------*/

/* USER CODE BEGIN Includes */

#include "string.h"

#include "stdio.h"

#include "stdlib.h"

#include "stdbool.h"

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/

/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/

/* USER CODE BEGIN PD */

#define pi 3.1415

#define p2r pi/1000

#define Kp 0.19

#define Ki 0.95

#define P 10.5

#define anpha 0.1

#define HILIM 100

#define LOLIM 0

#define Kb 5.263

#define Kd_p 0.112

#define Kp_p 0.56

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/

/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

TIM_HandleTypeDef htim2;

TIM_HandleTypeDef htim3;

TIM_HandleTypeDef htim4;

TIM_HandleTypeDef htim5;

UART_HandleTypeDef huart1;

/* USER CODE BEGIN PV */

int32_t PosCnt,Cnttmp,speed;

int16_t CountValue=0,RealVel,DesiredSpeed;

uint16_t AngPos1, AngPos0,CntVel;

uint8_t PreviousState,pwm,Speedmode,tick=0;

bool run=false, dir;

float CurPos=0,DesiredPos,CurVel;

char Rx_indx, Rx_Buffer[20],Rx_data[2];

const float sampleTime = 0.005;

double LinearPos;

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/

void SystemClock_Config(void);

static void MX_GPIO_Init(void);

static void MX_TIM2_Init(void);

static void MX_TIM3_Init(void);

static void MX_USART1_UART_Init(void);

static void MX_TIM4_Init(void);

static void MX_TIM5_Init(void);

/* USER CODE BEGIN PFP */

void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);

int SetVelLow(float CurrentPos, float Pos);

int SetVelMid(float CurrentPos, float Pos, float CurrentVel);

int SetVelHigh(float CurrentPos, float Pos, float CurrentVel);

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/

/* USER CODE BEGIN 0 */

#ifdef __GNUC__

#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)

#define GETCHAR_PROTOTYPE int fgetc(FILE *f)

#else

#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)

#define GETCHAR_PROTOTYPE int fgetc(FILE *f)

#endif

PUTCHAR_PROTOTYPE

{
 HAL_UART_Transmit(&huart1, (uint8_t*)&ch,1,100);

return ch;

// Ham ngat Uart

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)

uint8_t i;

if(huart->Instance == USART1) //uart1

if(Rx_indx==0) {for (i=0;i<20;i++) Rx_Buffer[i] =

0;}

switch(Rx_data[0]) {

/* dung dong co */

case 'e':

run =false;

break;

/* dong co chay */

case 'r':

run = true;

break;

case 'b':

// reset();

break;

case 's':
 DesiredPos = atoi(Rx_Buffer);

memset(Rx_Buffer, 0, sizeof(Rx_Buffer));

Rx_indx = 0;

break;

case 'v':

DesiredSpeed = atoi(Rx_Buffer);

memset(Rx_Buffer, 0, sizeof(Rx_Buffer));

Rx_indx = 0;

break;

case '0':

case '1':

case '2':

case '3':

case '4':

case '5':

case '6':

case '7':

case '8':

case '9':

case '.':

case '-':

Rx_Buffer[Rx_indx++] |= Rx_data[0];

break;

default:

break;

HAL_UART_Receive_IT(&huart1,(uint8_t*)Rx_data,1);
 }

void EXTI9_5_IRQHandler(void) // doc encoder

/* USER CODE BEGIN EXTI9_5_IRQn 0 */

unsigned char State0;

State0 = (State0<<1) | HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_4);

State0 = (State0<<1) | HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_6);

State0 = State0&0x03;

switch (State0) {

case 0:

if(PreviousState==1) CountValue++;

else CountValue--;

break;

case 1:

if(PreviousState==3) CountValue++;

else CountValue--;

break;

case 2:

if(PreviousState==0) CountValue++;

else CountValue--;

break;

case 3:

if(PreviousState==2) CountValue++;

else CountValue--;

break;
 }

PreviousState = State0;

CntVel++;

if (CountValue>=2000) {

CountValue = 0;

PosCnt++;

else if(CountValue<=-2000) {

CountValue = 0;

PosCnt--;

/* USER CODE END EXTI4_IRQn 0 */

HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_6);

/* USER CODE BEGIN EXTI4_IRQn 1 */

/**

* @brief This function handles EXTI line3 interrupt.

*/

void EXTI4_IRQHandler(void) // doc encoder

/* USER CODE BEGIN EXTI4_IRQn 0 */

// CHANNEL A

unsigned char State1;

State1 = (State1<<1) | HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_4);

State1 = (State1<<1) | HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_6);

State1 = State1&0x03;
switch (State1) {

case 0:

if(PreviousState==1) CountValue++;

else CountValue--;

break;

case 1:

if(PreviousState==3) CountValue++;

else CountValue--;

break;

case 2:

if(PreviousState==0) CountValue++;

else CountValue--;

break;

case 3:

if(PreviousState==2) CountValue++;

else CountValue--;

break;

PreviousState = State1;

CntVel++;

if (CountValue>=2000) {

CountValue = 0;

PosCnt++;

else if(CountValue<=-2000) {

CountValue = 0;

PosCnt--;
 }

/* USER CODE END EXTI3_IRQn 0 */

HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_4);

/* USER CODE BEGIN EXTI3_IRQn 1 */

/* USER CODE END EXTI3_IRQn 1 */

int PDPosctrl(float DesiredPos, float CurrentPos)

static float err_p = 0;

static float ud_p = 0;

float err, up, ud, ud_f, uout;

err = DesiredPos - CurrentPos;

up = Kp_p*err;

ud = Kd_p*(err - err_p)/sampleTime;

ud_f = (1-anpha)*ud_p + anpha*ud;

ud_p = ud_f;

err_p = err;

uout = up + ud_f;

return uout;

int PIVelctrl(float DesiredVel, float CurrentVel)

static float err_p = 0;

 static float ui_p = 0;

static float e_reset;

float err, up, ui,err_windup;

float uout;

float uout0;

int piout;

err = DesiredVel - CurrentVel;

up = Kp*err;

err_windup = Ki*err + Kb*e_reset;

ui = ui_p + err_windup*sampleTime;

ui_p = ui;

err_p = err;

uout = up+ui;

if(uout > HILIM)

uout0 = HILIM;

else if(uout < LOLIM)

uout0 = LOLIM;

else

uout0 = uout;

e_reset = uout0 - uout;

piout = (int)uout0;

return piout;

}
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { //
ngat timer 4 tinh van toc

if(htim->Instance==TIM4) // ngat do timer 4 5ms

CurPos = PosCnt*2*pi+CountValue*p2r; // Position calculation

rad

Cnttmp = CntVel;

CntVel = 0;

CurVel = Cnttmp*pi/5;
//rad/s

// STUDENTS HAVE TO CONVERT CURPOS (RAD) TO TRANSLATION

(MM)

LinearPos = (CurPos*P)/(2*pi);

// HAL_GPIO_WritePin(GPIOC,GPIO_PIN_3, GPIO_PIN_SET);

// __HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_2,100);

if ((run == 1) && (DesiredPos > LinearPos)) {

pwm = PIVelctrl(PDPosctrl(DesiredPos,LinearPos), CurVel);

HAL_GPIO_WritePin(GPIOC,GPIO_PIN_3, GPIO_PIN_SET);

__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_2,pwm);
// set pwm

else if (LinearPos > DesiredPos) {

pwm = PIVelctrl(PDPosctrl(LinearPos,DesiredPos), CurVel);

HAL_GPIO_WritePin(GPIOC,GPIO_PIN_3, GPIO_PIN_RESET);

__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_2,pwm); //
set pwm

if(run == 0)
 {

pwm = 0;

HAL_GPIO_WritePin(GPIOC,GPIO_PIN_3, GPIO_PIN_SET);

__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_2,pwm);

return;

if(htim->Instance==TIM5)

tick++;

if (run==0){

pwm = 0;

Speedmode =0;

else if ((run==1)&&(tick==5)){

tick=0;

printf("V%f\r \n",CurVel);

printf("P%f\r \n",LinearPos);

/* USER CODE END 0 */

/**

* @brief The application entry point.

 * @retval int

*/

int main(void)

/* USER CODE BEGIN 1 */

/* USER CODE END 1 */

/* MCU Configuration--------------------------------------------------------*/

/* Reset of all peripherals, Initializes the Flash interface and the Systick. */

HAL_Init();

/* USER CODE BEGIN Init */

/* USER CODE END Init */

/* Configure the system clock */

SystemClock_Config();

/* USER CODE BEGIN SysInit */

/* USER CODE END SysInit */

/* Initialize all configured peripherals */

MX_GPIO_Init();

MX_TIM2_Init();
 MX_TIM3_Init();

MX_USART1_UART_Init();

MX_TIM4_Init();

MX_TIM5_Init();

/* USER CODE BEGIN 2 */

HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3); // khoi tao timer 2

HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_2); // khoi tao timer 3

HAL_TIM_Base_Start_IT(&htim4); //
khoi tao timer 4

HAL_TIM_Base_Start_IT(&htim5); //
khoi tao timer 5

HAL_UART_Receive_IT(&huart1,(uint8_t*)Rx_data,1);

/* USER CODE END 2 */

/* Infinite loop */

/* USER CODE BEGIN WHILE */

while (1)

/* USER CODE END WHILE */

/* USER CODE BEGIN 3 */

/* USER CODE END 3 */

/**

* @brief System Clock Configuration

 * @retval None

*/

void SystemClock_Config(void)

RCC_OscInitTypeDef RCC_OscInitStruct = {0};

RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

/**Initializes the CPU, AHB and APB busses clocks

*/

RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;

RCC_OscInitStruct.HSIState = RCC_HSI_ON;

RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;

RCC_OscInitStruct.[Link] = RCC_PLL_ON;

RCC_OscInitStruct.[Link] = RCC_PLLSOURCE_HSI_DIV2;

RCC_OscInitStruct.[Link] = RCC_PLL_MUL12;

if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)

Error_Handler();

/**Initializes the CPU, AHB and APB busses clocks

*/

RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|
RCC_CLOCKTYPE_SYSCLK

|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;

RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;

RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;

RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
 RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV4;

if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1) !=
HAL_OK)

Error_Handler();

/**

* @brief TIM2 Initialization Function

* @param None

* @retval None

*/

static void MX_TIM2_Init(void)

/* USER CODE BEGIN TIM2_Init 0 */

/* USER CODE END TIM2_Init 0 */

TIM_MasterConfigTypeDef sMasterConfig = {0};

TIM_OC_InitTypeDef sConfigOC = {0};

/* USER CODE BEGIN TIM2_Init 1 */

/* USER CODE END TIM2_Init 1 */

 [Link] = TIM2;

[Link] = 11;

[Link] = TIM_COUNTERMODE_UP;

[Link] = 100;

[Link] = TIM_CLOCKDIVISION_DIV1;

[Link] = TIM_AUTORELOAD_PRELOAD_DISABLE;

if (HAL_TIM_PWM_Init(&htim2) != HAL_OK)

Error_Handler();

[Link] = TIM_TRGO_RESET;

[Link] = TIM_MASTERSLAVEMODE_DISABLE;

if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) !=
HAL_OK)

Error_Handler();

[Link] = TIM_OCMODE_PWM1;

[Link] = 0;

[Link] = TIM_OCPOLARITY_HIGH;

[Link] = TIM_OCFAST_DISABLE;

if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_3) !=

HAL_OK)

Error_Handler();

/* USER CODE BEGIN TIM2_Init 2 */

 /* USER CODE END TIM2_Init 2 */

HAL_TIM_MspPostInit(&htim2);

/**

* @brief TIM3 Initialization Function

* @param None

* @retval None

*/

static void MX_TIM3_Init(void)

/* USER CODE BEGIN TIM3_Init 0 */

/* USER CODE END TIM3_Init 0 */

TIM_MasterConfigTypeDef sMasterConfig = {0};

TIM_OC_InitTypeDef sConfigOC = {0};

/* USER CODE BEGIN TIM3_Init 1 */

/* USER CODE END TIM3_Init 1 */

[Link] = TIM3;

[Link] = 11;

[Link] = TIM_COUNTERMODE_UP;
 [Link] = 100;

[Link] = TIM_CLOCKDIVISION_DIV1;

[Link] = TIM_AUTORELOAD_PRELOAD_DISABLE;

if (HAL_TIM_PWM_Init(&htim3) != HAL_OK)

Error_Handler();

[Link] = TIM_TRGO_RESET;

[Link] = TIM_MASTERSLAVEMODE_DISABLE;

if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) !=
HAL_OK)

Error_Handler();

[Link] = TIM_OCMODE_PWM1;

[Link] = 0;

[Link] = TIM_OCPOLARITY_HIGH;

[Link] = TIM_OCFAST_DISABLE;

if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_2) !=

HAL_OK)

Error_Handler();

/* USER CODE BEGIN TIM3_Init 2 */

/* USER CODE END TIM3_Init 2 */

HAL_TIM_MspPostInit(&htim3);
}

/**

* @brief TIM4 Initialization Function

* @param None

* @retval None

*/

static void MX_TIM4_Init(void)

/* USER CODE BEGIN TIM4_Init 0 */

/* USER CODE END TIM4_Init 0 */

TIM_ClockConfigTypeDef sClockSourceConfig = {0};

TIM_MasterConfigTypeDef sMasterConfig = {0};

/* USER CODE BEGIN TIM4_Init 1 */

/* USER CODE END TIM4_Init 1 */

[Link] = TIM4;

[Link] = 23999;

[Link] = TIM_COUNTERMODE_UP;

[Link] = 4;

[Link] = TIM_CLOCKDIVISION_DIV1;

[Link] = TIM_AUTORELOAD_PRELOAD_DISABLE;
 if (HAL_TIM_Base_Init(&htim4) != HAL_OK)

Error_Handler();

[Link] = TIM_CLOCKSOURCE_INTERNAL;

if (HAL_TIM_ConfigClockSource(&htim4, &sClockSourceConfig) != HAL_OK)

Error_Handler();

[Link] = TIM_TRGO_RESET;

[Link] = TIM_MASTERSLAVEMODE_DISABLE;

if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) !=
HAL_OK)

Error_Handler();

/* USER CODE BEGIN TIM4_Init 2 */

/* USER CODE END TIM4_Init 2 */

/**

* @brief TIM5 Initialization Function

* @param None

* @retval None

*/
static void MX_TIM5_Init(void)

/* USER CODE BEGIN TIM5_Init 0 */

/* USER CODE END TIM5_Init 0 */

TIM_ClockConfigTypeDef sClockSourceConfig = {0};

TIM_MasterConfigTypeDef sMasterConfig = {0};

/* USER CODE BEGIN TIM5_Init 1 */

/* USER CODE END TIM5_Init 1 */

[Link] = TIM5;

[Link] = 23999;

[Link] = TIM_COUNTERMODE_UP;

[Link] = 9;

[Link] = TIM_CLOCKDIVISION_DIV1;

[Link] = TIM_AUTORELOAD_PRELOAD_DISABLE;

if (HAL_TIM_Base_Init(&htim5) != HAL_OK)

Error_Handler();

[Link] = TIM_CLOCKSOURCE_INTERNAL;

if (HAL_TIM_ConfigClockSource(&htim5, &sClockSourceConfig) != HAL_OK)

Error_Handler();
 }

[Link] = TIM_TRGO_RESET;

[Link] = TIM_MASTERSLAVEMODE_DISABLE;

if (HAL_TIMEx_MasterConfigSynchronization(&htim5, &sMasterConfig) !=
HAL_OK)

Error_Handler();

/* USER CODE BEGIN TIM5_Init 2 */

/* USER CODE END TIM5_Init 2 */

/**

* @brief USART1 Initialization Function

* @param None

* @retval None

*/

static void MX_USART1_UART_Init(void)

/* USER CODE BEGIN USART1_Init 0 */

/* USER CODE END USART1_Init 0 */

/* USER CODE BEGIN USART1_Init 1 */

 /* USER CODE END USART1_Init 1 */

[Link] = USART1;

[Link] = 115200;

[Link] = UART_WORDLENGTH_8B;

[Link] = UART_STOPBITS_1;

[Link] = UART_PARITY_NONE;

[Link] = UART_MODE_TX_RX;

[Link] = UART_HWCONTROL_NONE;

[Link] = UART_OVERSAMPLING_16;

if (HAL_UART_Init(&huart1) != HAL_OK)

Error_Handler();

/* USER CODE BEGIN USART1_Init 2 */

/* USER CODE END USART1_Init 2 */

/**

* @brief GPIO Initialization Function

* @param None

* @retval None

*/

static void MX_GPIO_Init(void)

{
GPIO_InitTypeDef GPIO_InitStruct = {0};

/* GPIO Ports Clock Enable */

__HAL_RCC_GPIOC_CLK_ENABLE();

__HAL_RCC_GPIOB_CLK_ENABLE();

__HAL_RCC_GPIOA_CLK_ENABLE();

/*Configure GPIO pin Output Level */

HAL_GPIO_WritePin(GPIOC, GPIO_PIN_3, GPIO_PIN_RESET);

/*Configure GPIO pin : PC3 */

GPIO_InitStruct.Pin = GPIO_PIN_3;

GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;

GPIO_InitStruct.Pull = GPIO_NOPULL;

GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;

HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);

/*Configure GPIO pins : PB4 PB6 */

GPIO_InitStruct.Pin = GPIO_PIN_4|GPIO_PIN_6;

GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;

GPIO_InitStruct.Pull = GPIO_PULLUP;

HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

/* EXTI interrupt init*/

HAL_NVIC_SetPriority(EXTI4_IRQn, 0, 0);

HAL_NVIC_EnableIRQ(EXTI4_IRQn);
 HAL_NVIC_SetPriority(EXTI9_5_IRQn, 0, 0);

HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**

* @brief This function is executed in case of error occurrence.

* @retval None

*/

void Error_Handler(void)

/* USER CODE BEGIN Error_Handler_Debug */

/* User can add his own implementation to report the HAL error return state
*/

/* USER CODE END Error_Handler_Debug */

#ifdef USE_FULL_ASSERT

/**

* @brief Reports the name of the source file and the source line number

* where the assert_param error has occurred.

* @param file: pointer to the source file name

 * @param line: assert_param error line source number

* @retval None

*/

void assert_failed(uint8_t *file, uint32_t line)

/* USER CODE BEGIN 6 */

/* User can add his own implementation to report the file name and line
number,

tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */

/* USER CODE END 6 */

#endif /* USE_FULL_ASSERT */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF

FILE****/

VELOCITY

/* USER CODE BEGIN Header */

/**

******************************************************************************

* @file : main.c

* @brief : Main program body

******************************************************************************

* @attention

* <h2><center>&copy; Copyright (c) 2025 STMicroelectronics.

* All rights reserved.</center></h2>

* This software component is licensed by ST under BSD 3-Clause license,

* the "License"; You may not use this file except in compliance with the

* License. You may obtain a copy of the License at:

* [Link]/licenses/BSD-3-Clause

******************************************************************************

*/

/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/

#include "main.h"

/* Private includes ----------------------------------------------------------*/

/* USER CODE BEGIN Includes */

#include "string.h"

#include "stdio.h"

#include "stdlib.h"

#include "stdbool.h"

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/

/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/

/* USER CODE BEGIN PD */

#define pi 3.1415

#define p2r pi/2000

#define HILIM 100

#define LOLIM 0

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/

/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

TIM_HandleTypeDef htim3;

TIM_HandleTypeDef htim4;

TIM_HandleTypeDef htim5;

UART_HandleTypeDef huart1;

/* USER CODE BEGIN PV */

int32_t PosCnt,Cnttmp,speed;
int16_t CountValue=0,RealVel,DesiredSpeed;

uint16_t AngPos1, AngPos0,CntVel;

uint8_t PreviousState,pwm,Speedmode,tick=0;

bool run=false, dir;

float CurPos=0,DesiredPos,CurVel;

char Rx_indx, Rx_Buffer[20],Rx_data[2];

float Kp=0.22016;

float Ki=3.19656;

float Kb=14.5193;

float sampleTime= 0.005;

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/

void SystemClock_Config(void);

static void MX_GPIO_Init(void);

static void MX_TIM3_Init(void);

static void MX_TIM4_Init(void);

static void MX_TIM5_Init(void);

static void MX_USART1_UART_Init(void);

/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/

/* USER CODE BEGIN 0 */

#ifdef __GNUC__
 #define PUTCHAR_PROTOTYPE int __io_putchar(int ch)

#else

#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)

#define GETCHAR_PROTOTYPE int fgetc(FILE *f)

#endif

PUTCHAR_PROTOTYPE

HAL_UART_Transmit(&huart1, (uint8_t*)&ch,1,100);

return ch;

// Ham ngat Uart

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)

uint8_t i;

if(huart->Instance == USART1) //uart1

if(Rx_indx==0) {for (i=0;i<20;i++) Rx_Buffer[i] =

0;}

switch(Rx_data[0]) {

/* dung dong co */

case 'e':

run =false;

break;

/* dong co chay */
 case 'r':

run = true;

break;

case 'b':

// reset();

break;

case 's':

DesiredPos = atoi(Rx_Buffer);

memset(Rx_Buffer, 0, sizeof(Rx_Buffer));

Rx_indx = 0;

break;

case 'v':

DesiredSpeed = atoi(Rx_Buffer);

memset(Rx_Buffer, 0, sizeof(Rx_Buffer));

Rx_indx = 0;

break;

case '0':

case '1':

case '2':

case '3':

case '4':

case '5':

case '6':

case '7':

case '8':

case '9':

case '.':
 case '-':

Rx_Buffer[Rx_indx++] |= Rx_data[0];

break;

default:

break;

HAL_UART_Receive_IT(&huart1,(uint8_t*)Rx_data,1);

void EXTI9_5_IRQHandler(void) // doc encoder

/* USER CODE BEGIN EXTI9_5_IRQn 0 */

unsigned char State0;

State0 = (State0<<1) | HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_4);

State0 = (State0<<1) | HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_6);

State0 = State0&0x03;

switch (State0) {

case 0:

if(PreviousState==1) CountValue++;

else CountValue--;

break;

case 1:

if(PreviousState==3) CountValue++;

else CountValue--;

break;

case 2:
 if(PreviousState==0) CountValue++;

else CountValue--;

break;

case 3:

if(PreviousState==2) CountValue++;

else CountValue--;

break;

PreviousState = State0;

CntVel++;

if (CountValue>=4000) {

CountValue = 0;

PosCnt++;

else if(CountValue<=-4000) {

CountValue = 0;

PosCnt--;

/* USER CODE END EXTI4_IRQn 0 */

HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_6);

/* USER CODE BEGIN EXTI4_IRQn 1 */

/**

* @brief This function handles EXTI line3 interrupt.

*/

void EXTI4_IRQHandler(void) // doc encoder

{

/* USER CODE BEGIN EXTI4_IRQn 0 */

// CHANNEL A

unsigned char State1;

State1 = (State1<<1) | HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_4);

State1 = (State1<<1) | HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_6);

State1 = State1&0x03;

switch (State1) {

case 0:

if(PreviousState==1) CountValue++;

else CountValue--;

break;

case 1:

if(PreviousState==3) CountValue++;

else CountValue--;

break;

case 2:

if(PreviousState==0) CountValue++;

else CountValue--;

break;

case 3:

if(PreviousState==2) CountValue++;

else CountValue--;

break;

PreviousState = State1;

CntVel++;
 if (CountValue>=4000) {

CountValue = 0;

PosCnt++;

else if(CountValue<=-4000) {

CountValue = 0;

PosCnt--;

/* USER CODE END EXTI3_IRQn 0 */

HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_4);

/* USER CODE BEGIN EXTI3_IRQn 1 */

/* USER CODE END EXTI3_IRQn 1 */

int PI(float Desired, float Current)

static float err_reset=0;

static float ui_p=0;

float err, up, ui;

float uouthat;

float uout;

int piout;

err= Desired-Current;

up= err*Kp;

ui= ui_p+Ki*err*sampleTime+Kb*sampleTime*err_reset;

//err_p=err;

ui_p=ui;
 uout= up+ui;

if( uout>HILIM)

uouthat=HILIM;

else if (uout<LOLIM)

uouthat= LOLIM;

else uouthat= uout;

err_reset= uouthat-uout;

piout= (int)(uouthat);

return piout;

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { //

ngat timer 4 tinh van toc

if(htim->Instance==TIM4) // ngat do timer 4 5ms

CurPos = PosCnt*2*pi+CountValue*p2r; // Position calculation

rad

Cnttmp = CntVel;

CntVel = 0;

RealVel = Cnttmp*3;
//RPM

CurVel = Cnttmp*pi/10;
//rad/s

pwm=PI(DesiredSpeed,CurVel);
 if (run==1)

HAL_GPIO_WritePin(GPIOC,GPIO_PIN_3,
GPIO_PIN_SET);

__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_2,pwm);
// set pwm

else if( run==0)

HAL_GPIO_WritePin(GPIOC,GPIO_PIN_3,
GPIO_PIN_RESET);

__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_2,pwm); //
set pwm

return;

// if(htim->Instance==TIM5)

// {

// tick++;

// if (run==0){

// pwm = 0;

// Speedmode =0;

// }

// else if ((run==1)&&(tick==5)){

// tick=0;

// printf("V%f\r \n",CurVel);

// printf("P%f\r \n",CurPos);

// }
// }

/* USER CODE END 0 */

/**

* @brief The application entry point.

* @retval int

*/

int main(void)

/* USER CODE BEGIN 1 */

/* USER CODE END 1 */

/* MCU Configuration--------------------------------------------------------*/

/* Reset of all peripherals, Initializes the Flash interface and the Systick. */

HAL_Init();

/* USER CODE BEGIN Init */

/* USER CODE END Init */

/* Configure the system clock */

SystemClock_Config();
 /* USER CODE BEGIN SysInit */

/* USER CODE END SysInit */

/* Initialize all configured peripherals */

MX_GPIO_Init();

MX_TIM3_Init();

MX_TIM4_Init();

MX_TIM5_Init();

MX_USART1_UART_Init();

/* USER CODE BEGIN 2 */

HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_2); // khoi tao timer 3

HAL_TIM_Base_Start_IT(&htim4); //
khoi tao timer 4

HAL_TIM_Base_Start_IT(&htim5); //
khoi tao timer 5

HAL_UART_Receive_IT(&huart1,(uint8_t*)Rx_data,1);

/* USER CODE END 2 */

/* Infinite loop */

/* USER CODE BEGIN WHILE */

while (1)

if (run==1)

//tick=0;
 printf("V%f\r \n",CurVel);

printf("P%f\r \n",CurPos);

HAL_Delay(20);

/* USER CODE END WHILE */

/* USER CODE BEGIN 3 */

/* USER CODE END 3 */

/**

* @brief System Clock Configuration

* @retval None

*/

void SystemClock_Config(void)

RCC_OscInitTypeDef RCC_OscInitStruct = {0};

RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

/** Initializes the RCC Oscillators according to the specified parameters

* in the RCC_OscInitTypeDef structure.

*/

RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;

RCC_OscInitStruct.HSIState = RCC_HSI_ON;

RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;

RCC_OscInitStruct.[Link] = RCC_PLL_ON;
 RCC_OscInitStruct.[Link] = RCC_PLLSOURCE_HSI_DIV2;

RCC_OscInitStruct.[Link] = RCC_PLL_MUL12;

if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)

Error_Handler();

/** Initializes the CPU, AHB and APB buses clocks

*/

RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|
RCC_CLOCKTYPE_SYSCLK

|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;

RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;

RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;

RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;

RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV4;

if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1) !=
HAL_OK)

Error_Handler();

/**

* @brief TIM3 Initialization Function

* @param None

* @retval None
 */

static void MX_TIM3_Init(void)

/* USER CODE BEGIN TIM3_Init 0 */

/* USER CODE END TIM3_Init 0 */

TIM_MasterConfigTypeDef sMasterConfig = {0};

TIM_OC_InitTypeDef sConfigOC = {0};

/* USER CODE BEGIN TIM3_Init 1 */

/* USER CODE END TIM3_Init 1 */

[Link] = TIM3;

[Link] = 11;

[Link] = TIM_COUNTERMODE_UP;

[Link] = 100;

[Link] = TIM_CLOCKDIVISION_DIV1;

[Link] = TIM_AUTORELOAD_PRELOAD_DISABLE;

if (HAL_TIM_PWM_Init(&htim3) != HAL_OK)

Error_Handler();

[Link] = TIM_TRGO_RESET;

[Link] = TIM_MASTERSLAVEMODE_DISABLE;
 if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) !=
HAL_OK)

Error_Handler();

[Link] = TIM_OCMODE_PWM1;

[Link] = 0;

[Link] = TIM_OCPOLARITY_HIGH;

[Link] = TIM_OCFAST_DISABLE;

if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_2) !=

HAL_OK)

Error_Handler();

/* USER CODE BEGIN TIM3_Init 2 */

/* USER CODE END TIM3_Init 2 */

HAL_TIM_MspPostInit(&htim3);

/**

* @brief TIM4 Initialization Function

* @param None

* @retval None

*/

static void MX_TIM4_Init(void)

{

/* USER CODE BEGIN TIM4_Init 0 */

/* USER CODE END TIM4_Init 0 */

TIM_ClockConfigTypeDef sClockSourceConfig = {0};

TIM_MasterConfigTypeDef sMasterConfig = {0};

/* USER CODE BEGIN TIM4_Init 1 */

/* USER CODE END TIM4_Init 1 */

[Link] = TIM4;

[Link] = 23999;

[Link] = TIM_COUNTERMODE_UP;

[Link] = 4;

[Link] = TIM_CLOCKDIVISION_DIV1;

[Link] = TIM_AUTORELOAD_PRELOAD_DISABLE;

if (HAL_TIM_Base_Init(&htim4) != HAL_OK)

Error_Handler();

[Link] = TIM_CLOCKSOURCE_INTERNAL;

if (HAL_TIM_ConfigClockSource(&htim4, &sClockSourceConfig) != HAL_OK)

Error_Handler();

}
 [Link] = TIM_TRGO_RESET;

[Link] = TIM_MASTERSLAVEMODE_DISABLE;

if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) !=
HAL_OK)

Error_Handler();

/* USER CODE BEGIN TIM4_Init 2 */

/* USER CODE END TIM4_Init 2 */

/**

* @brief TIM5 Initialization Function

* @param None

* @retval None

*/

static void MX_TIM5_Init(void)

/* USER CODE BEGIN TIM5_Init 0 */

/* USER CODE END TIM5_Init 0 */

TIM_ClockConfigTypeDef sClockSourceConfig = {0};

TIM_MasterConfigTypeDef sMasterConfig = {0};

 /* USER CODE BEGIN TIM5_Init 1 */

/* USER CODE END TIM5_Init 1 */

[Link] = TIM5;

[Link] = 23999;

[Link] = TIM_COUNTERMODE_UP;

[Link] = 9;

[Link] = TIM_CLOCKDIVISION_DIV1;

[Link] = TIM_AUTORELOAD_PRELOAD_DISABLE;

if (HAL_TIM_Base_Init(&htim5) != HAL_OK)

Error_Handler();

[Link] = TIM_CLOCKSOURCE_INTERNAL;

if (HAL_TIM_ConfigClockSource(&htim5, &sClockSourceConfig) != HAL_OK)

Error_Handler();

[Link] = TIM_TRGO_RESET;

[Link] = TIM_MASTERSLAVEMODE_DISABLE;

if (HAL_TIMEx_MasterConfigSynchronization(&htim5, &sMasterConfig) !=
HAL_OK)

Error_Handler();

/* USER CODE BEGIN TIM5_Init 2 */

 /* USER CODE END TIM5_Init 2 */

/**

* @brief USART1 Initialization Function

* @param None

* @retval None

*/

static void MX_USART1_UART_Init(void)

/* USER CODE BEGIN USART1_Init 0 */

/* USER CODE END USART1_Init 0 */

/* USER CODE BEGIN USART1_Init 1 */

/* USER CODE END USART1_Init 1 */

[Link] = USART1;

[Link] = 115200;

[Link] = UART_WORDLENGTH_8B;

[Link] = UART_STOPBITS_1;

[Link] = UART_PARITY_NONE;

[Link] = UART_MODE_TX_RX;

[Link] = UART_HWCONTROL_NONE;
 [Link] = UART_OVERSAMPLING_16;

if (HAL_UART_Init(&huart1) != HAL_OK)

Error_Handler();

/* USER CODE BEGIN USART1_Init 2 */

/* USER CODE END USART1_Init 2 */

/**

* @brief GPIO Initialization Function

* @param None

* @retval None

*/

static void MX_GPIO_Init(void)

GPIO_InitTypeDef GPIO_InitStruct = {0};

/* GPIO Ports Clock Enable */

__HAL_RCC_GPIOC_CLK_ENABLE();

__HAL_RCC_GPIOA_CLK_ENABLE();

__HAL_RCC_GPIOB_CLK_ENABLE();

/*Configure GPIO pin Output Level */

HAL_GPIO_WritePin(GPIOC, GPIO_PIN_3, GPIO_PIN_RESET);

 /*Configure GPIO pin : PC3 */

GPIO_InitStruct.Pin = GPIO_PIN_3;

GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;

GPIO_InitStruct.Pull = GPIO_NOPULL;

GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;

HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);

/*Configure GPIO pins : PB4 PB6 */

GPIO_InitStruct.Pin = GPIO_PIN_4|GPIO_PIN_6;

GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;

GPIO_InitStruct.Pull = GPIO_PULLUP;

HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

/* EXTI interrupt init*/

HAL_NVIC_SetPriority(EXTI4_IRQn, 0, 0);

HAL_NVIC_EnableIRQ(EXTI4_IRQn);

HAL_NVIC_SetPriority(EXTI9_5_IRQn, 0, 0);

HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**

* @brief This function is executed in case of error occurrence.

* @retval None

*/

void Error_Handler(void)

/* USER CODE BEGIN Error_Handler_Debug */

/* User can add his own implementation to report the HAL error return state
*/

__disable_irq();

while (1)

/* USER CODE END Error_Handler_Debug */

#ifdef USE_FULL_ASSERT

/**

* @brief Reports the name of the source file and the source line number

* where the assert_param error has occurred.

* @param file: pointer to the source file name

* @param line: assert_param error line source number

* @retval None

*/

void assert_failed(uint8_t *file, uint32_t line)

/* USER CODE BEGIN 6 */

 /* User can add his own implementation to report the file name and line
number,

ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */

/* USER CODE END 6 */

#endif /* USE_FULL_ASSERT */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF

FILE****/