mirror of
https://github.com/DashyFox/StackSport.git
synced 2025-05-04 07:10:17 +00:00
683 lines
18 KiB
C
683 lines
18 KiB
C
/* USER CODE BEGIN Header */
|
||
/**
|
||
******************************************************************************
|
||
* @file stm32f1xx_it.c
|
||
* @brief Interrupt Service Routines.
|
||
******************************************************************************
|
||
* @attention
|
||
*
|
||
* Copyright (c) 2024 STMicroelectronics.
|
||
* All rights reserved.
|
||
*
|
||
* This software is licensed under terms that can be found in the LICENSE file
|
||
* in the root directory of this software component.
|
||
* If no LICENSE file comes with this software, it is provided AS-IS.
|
||
*
|
||
******************************************************************************
|
||
*/
|
||
/* USER CODE END Header */
|
||
|
||
/* Includes ------------------------------------------------------------------*/
|
||
#include "main.h"
|
||
#include "stm32f1xx_it.h"
|
||
/* Private includes ----------------------------------------------------------*/
|
||
/* USER CODE BEGIN Includes */
|
||
#include "pca9685.h"
|
||
#include "IR.h"
|
||
|
||
#include "usb_device.h"
|
||
#include "usbd_cdc_if.h"
|
||
|
||
#include "GlobalDefines.h"
|
||
/* USER CODE END Includes */
|
||
|
||
/* Private typedef -----------------------------------------------------------*/
|
||
/* USER CODE BEGIN TD */
|
||
|
||
/* USER CODE END TD */
|
||
|
||
/* Private define ------------------------------------------------------------*/
|
||
/* USER CODE BEGIN PD */
|
||
|
||
/* USER CODE END PD */
|
||
|
||
/* Private macro -------------------------------------------------------------*/
|
||
/* USER CODE BEGIN PM */
|
||
|
||
/* USER CODE END PM */
|
||
|
||
/* Private variables ---------------------------------------------------------*/
|
||
/* USER CODE BEGIN PV */
|
||
|
||
uint8_t myi;
|
||
// uint8_t ticktime = 0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>-<2D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> 0,01 <20>
|
||
uint8_t update1 = 0; // <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 0 <20><><EFBFBD> 1
|
||
uint8_t update2 = 0;
|
||
extern uint16_t vi; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><> 0..599
|
||
uint16_t v1[30]; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> (<28><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>) [0] - <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, [1-29] - <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
uint16_t v2[30];
|
||
|
||
uint32_t vt1 = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> (<28><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>) (<28><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>)
|
||
uint32_t vt2 = 0;
|
||
|
||
uint16_t vs1[4] = {0, 0, 0, 0}; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> 3 <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
uint16_t vs2[4] = {0, 0, 0, 0}; //[0] - <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
//[1-3] - <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> vt
|
||
|
||
uint32_t vsk1 = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> (<28><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>)
|
||
uint32_t vsk2 = 0;
|
||
|
||
uint16_t Vzad1 = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
uint16_t Vupr1 = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
|
||
uint16_t Vzad2 = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
uint16_t Vupr2 = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
|
||
extern uint8_t rejim[60]; // 0 - chislo rejimov, 1-6 - rejim1, 7-12 - rejim2...
|
||
extern uint8_t avto; // vkl/otkl avtomaticheskoi raboti
|
||
extern uint8_t rejim_number; // nomer tekyshego rejima
|
||
|
||
extern uint8_t initcomlete;
|
||
extern uint16_t timing1; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
extern uint16_t timing2; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
extern uint8_t velosety[600]; // <20><><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><>
|
||
|
||
extern uint8_t Vz1;
|
||
extern uint8_t Vz2;
|
||
|
||
/* USER CODE END PV */
|
||
|
||
/* Private function prototypes -----------------------------------------------*/
|
||
/* USER CODE BEGIN PFP */
|
||
int32_t computePID1(uint16_t input, uint16_t setpoint);
|
||
int32_t computePID2(uint16_t input, uint16_t setpoint);
|
||
/* USER CODE END PFP */
|
||
|
||
/* Private user code ---------------------------------------------------------*/
|
||
/* USER CODE BEGIN 0 */
|
||
int32_t computePID1(uint16_t input, uint16_t setpoint)
|
||
{
|
||
static int32_t integral = 0, prevErr = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
|
||
int32_t err = setpoint - input; // 1.<2E><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
|
||
integral = integral + err / 20; // 2.<2E><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> dt = 0.01, ki = 5;
|
||
|
||
if (integral > 4000)
|
||
{
|
||
integral = 4000;
|
||
}
|
||
if (integral < 0)
|
||
{
|
||
integral = 0;
|
||
}
|
||
|
||
int32_t D = (err - prevErr); // 3.<2E><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> dt = 0.01, kd = 0.01
|
||
if (D > 4000)
|
||
{
|
||
D = 4000;
|
||
}
|
||
if (D < 0)
|
||
{
|
||
D = 0;
|
||
}
|
||
|
||
prevErr = err;
|
||
//
|
||
int32_t Rout = ((err * 3) >> 1) + integral + D; // 4. <20>=1,5 + <20>=5 + <20>=0.01
|
||
if (Rout > 4000)
|
||
{
|
||
Rout = 4000;
|
||
} //
|
||
if (Rout < 0)
|
||
{
|
||
Rout = 0;
|
||
} //
|
||
|
||
return Rout;
|
||
}
|
||
|
||
int32_t computePID2(uint16_t input, uint16_t setpoint)
|
||
{
|
||
static int32_t integral = 0, prevErr = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
int32_t err = setpoint - input; // 1.<2E><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
|
||
integral = integral + err / 20; // 2.<2E><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> dt = 0.01, ki = 5;
|
||
if (integral > 4000)
|
||
{
|
||
integral = 4000;
|
||
}
|
||
if (integral < 0)
|
||
{
|
||
integral = 0;
|
||
}
|
||
|
||
int32_t D = (err - prevErr); // 3.<2E><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> dt = 0.01, kd = 0.01
|
||
if (D > 4000)
|
||
{
|
||
D = 4000;
|
||
}
|
||
if (D < 0)
|
||
{
|
||
D = 0;
|
||
}
|
||
|
||
prevErr = err;
|
||
//
|
||
int32_t Rout = ((err * 3) >> 1) + integral + D; // 4. <20>=1,5 + <20>=5 + <20>=0.01
|
||
if (Rout > 4000)
|
||
{
|
||
Rout = 4000;
|
||
} //
|
||
if (Rout < 0)
|
||
{
|
||
Rout = 0;
|
||
} //
|
||
|
||
return Rout;
|
||
}
|
||
/* USER CODE END 0 */
|
||
|
||
/* External variables --------------------------------------------------------*/
|
||
extern PCD_HandleTypeDef hpcd_USB_FS;
|
||
extern I2C_HandleTypeDef hi2c1;
|
||
extern TIM_HandleTypeDef htim3;
|
||
extern UART_HandleTypeDef huart3;
|
||
/* USER CODE BEGIN EV */
|
||
extern uint8_t uart_rx_buffer[UART_BUFFER_SIZE];
|
||
/* USER CODE END EV */
|
||
|
||
/******************************************************************************/
|
||
/* Cortex-M3 Processor Interruption and Exception Handlers */
|
||
/******************************************************************************/
|
||
/**
|
||
* @brief This function handles Non maskable interrupt.
|
||
*/
|
||
void NMI_Handler(void)
|
||
{
|
||
/* USER CODE BEGIN NonMaskableInt_IRQn 0 */
|
||
|
||
/* USER CODE END NonMaskableInt_IRQn 0 */
|
||
/* USER CODE BEGIN NonMaskableInt_IRQn 1 */
|
||
while (1)
|
||
{
|
||
}
|
||
/* USER CODE END NonMaskableInt_IRQn 1 */
|
||
}
|
||
|
||
/**
|
||
* @brief This function handles Hard fault interrupt.
|
||
*/
|
||
void HardFault_Handler(void)
|
||
{
|
||
/* USER CODE BEGIN HardFault_IRQn 0 */
|
||
|
||
/* USER CODE END HardFault_IRQn 0 */
|
||
while (1)
|
||
{
|
||
/* USER CODE BEGIN W1_HardFault_IRQn 0 */
|
||
/* USER CODE END W1_HardFault_IRQn 0 */
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief This function handles Memory management fault.
|
||
*/
|
||
void MemManage_Handler(void)
|
||
{
|
||
/* USER CODE BEGIN MemoryManagement_IRQn 0 */
|
||
|
||
/* USER CODE END MemoryManagement_IRQn 0 */
|
||
while (1)
|
||
{
|
||
/* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */
|
||
/* USER CODE END W1_MemoryManagement_IRQn 0 */
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief This function handles Prefetch fault, memory access fault.
|
||
*/
|
||
void BusFault_Handler(void)
|
||
{
|
||
/* USER CODE BEGIN BusFault_IRQn 0 */
|
||
|
||
/* USER CODE END BusFault_IRQn 0 */
|
||
while (1)
|
||
{
|
||
/* USER CODE BEGIN W1_BusFault_IRQn 0 */
|
||
/* USER CODE END W1_BusFault_IRQn 0 */
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief This function handles Undefined instruction or illegal state.
|
||
*/
|
||
void UsageFault_Handler(void)
|
||
{
|
||
/* USER CODE BEGIN UsageFault_IRQn 0 */
|
||
|
||
/* USER CODE END UsageFault_IRQn 0 */
|
||
while (1)
|
||
{
|
||
/* USER CODE BEGIN W1_UsageFault_IRQn 0 */
|
||
/* USER CODE END W1_UsageFault_IRQn 0 */
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief This function handles System service call via SWI instruction.
|
||
*/
|
||
void SVC_Handler(void)
|
||
{
|
||
/* USER CODE BEGIN SVCall_IRQn 0 */
|
||
|
||
/* USER CODE END SVCall_IRQn 0 */
|
||
/* USER CODE BEGIN SVCall_IRQn 1 */
|
||
|
||
/* USER CODE END SVCall_IRQn 1 */
|
||
}
|
||
|
||
/**
|
||
* @brief This function handles Debug monitor.
|
||
*/
|
||
void DebugMon_Handler(void)
|
||
{
|
||
/* USER CODE BEGIN DebugMonitor_IRQn 0 */
|
||
|
||
/* USER CODE END DebugMonitor_IRQn 0 */
|
||
/* USER CODE BEGIN DebugMonitor_IRQn 1 */
|
||
|
||
/* USER CODE END DebugMonitor_IRQn 1 */
|
||
}
|
||
|
||
/**
|
||
* @brief This function handles Pendable request for system service.
|
||
*/
|
||
void PendSV_Handler(void)
|
||
{
|
||
/* USER CODE BEGIN PendSV_IRQn 0 */
|
||
|
||
/* USER CODE END PendSV_IRQn 0 */
|
||
/* USER CODE BEGIN PendSV_IRQn 1 */
|
||
|
||
/* USER CODE END PendSV_IRQn 1 */
|
||
}
|
||
|
||
/**
|
||
* @brief This function handles System tick timer.
|
||
*/
|
||
void SysTick_Handler(void)
|
||
{
|
||
/* USER CODE BEGIN SysTick_IRQn 0 */
|
||
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 1
|
||
if (v1[0] > 0)
|
||
{ // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
vt1 = 0;
|
||
for (uint8_t i = 1; i <= v1[0]; i++)
|
||
{
|
||
vt1 += v1[i];
|
||
}
|
||
vt1 = vt1 / v1[0];
|
||
// <20><><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>, <20><> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
}
|
||
else
|
||
{
|
||
vt1 = timing1;
|
||
}
|
||
v1[0] = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 2
|
||
if (v2[0] > 0)
|
||
{ // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
vt2 = 0;
|
||
for (uint8_t i = 1; i <= v2[0]; i++)
|
||
{
|
||
vt2 += v2[i];
|
||
}
|
||
vt2 = vt2 / v2[0];
|
||
// <20><><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>, <20><> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
}
|
||
else
|
||
{
|
||
vt2 = timing2;
|
||
}
|
||
v2[0] = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> 3
|
||
if (vs1[0] < 3)
|
||
{
|
||
vs1[vs1[0] + 1] = (uint16_t)vt1;
|
||
vs1[0]++;
|
||
}
|
||
else
|
||
{
|
||
vs1[0] = 0;
|
||
vs1[vs1[0] + 1] = (uint16_t)vt1;
|
||
vs1[0]++;
|
||
}
|
||
|
||
vsk1 = (vs1[1] + vs1[2] + vs1[3]) / 3;
|
||
|
||
if (vs2[0] < 3)
|
||
{
|
||
vs2[vs2[0] + 1] = (uint16_t)vt2;
|
||
vs2[0]++;
|
||
}
|
||
else
|
||
{
|
||
vs2[0] = 0;
|
||
vs2[vs2[0] + 1] = (uint16_t)vt2;
|
||
vs2[0]++;
|
||
}
|
||
|
||
vsk2 = (vs2[1] + vs2[2] + vs2[3]) / 3;
|
||
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 60*1000000/(<28>*10)
|
||
if (vsk1 != 0)
|
||
{
|
||
vsk1 = 6000000 / vsk1;
|
||
}
|
||
else
|
||
{
|
||
vsk1 = 0;
|
||
}
|
||
if (vsk1 > 10000) vsk1 = 10000;
|
||
if (vsk1 < 150) vsk1 = 0;
|
||
|
||
if (vsk2 != 0)
|
||
{
|
||
vsk2 = 6000000 / vsk2;
|
||
}
|
||
else
|
||
{
|
||
vsk2 = 0;
|
||
}
|
||
if (vsk2 > 10000)
|
||
vsk2 = 10000;
|
||
if (vsk2 < 150)
|
||
vsk2 = 0;
|
||
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 0 - 200 <20> -8000 ... 8000 <20><>./<2F><><EFBFBD>.
|
||
if (Vz1 > 100)
|
||
{
|
||
Vzad1 = (Vz1 - 100) * 80;
|
||
}
|
||
else
|
||
{
|
||
Vzad1 = (100 - Vz1) * 80;
|
||
}
|
||
if (Vz2 > 100)
|
||
{
|
||
Vzad2 = (Vz2 - 100) * 80;
|
||
}
|
||
else
|
||
{
|
||
Vzad2 = (100 - Vz2) * 80;
|
||
}
|
||
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
Vupr1 = (uint16_t)computePID1(vsk1, Vzad1);
|
||
Vupr2 = (uint16_t)computePID2(vsk2, Vzad2);
|
||
|
||
if (Vz1 > 100)
|
||
{
|
||
TIM2->CCR2 = 0;
|
||
TIM2->CCR1 = Vupr1;
|
||
}
|
||
else
|
||
{
|
||
TIM2->CCR1 = 0;
|
||
TIM2->CCR2 = Vupr1;
|
||
}
|
||
|
||
if (Vz2 > 100)
|
||
{
|
||
TIM2->CCR3 = 0;
|
||
TIM2->CCR4 = Vupr2;
|
||
}
|
||
else
|
||
{
|
||
TIM2->CCR4 = 0;
|
||
TIM2->CCR3 = Vupr2;
|
||
}
|
||
|
||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><>
|
||
if (vi < 600)
|
||
{
|
||
velosety[vi] = LOBYTE(timing1);
|
||
velosety[vi + 1] = HIBYTE(timing1);
|
||
velosety[vi + 2] = LOBYTE(vsk1);
|
||
velosety[vi + 3] = HIBYTE(vsk1);
|
||
velosety[vi + 4] = LOBYTE(vsk2);
|
||
velosety[vi + 5] = HIBYTE(vsk2);
|
||
vi = vi + 6;
|
||
}
|
||
|
||
// else
|
||
// {vi = 0;
|
||
// velosety[vi] = LOBYTE(timing1);
|
||
// velosety[vi+1] = HIBYTE(timing1);
|
||
// velosety[vi+2] = LOBYTE(vt);
|
||
// velosety[vi+3] = HIBYTE(vt);
|
||
// velosety[vi+4] = LOBYTE(vsk);
|
||
// velosety[vi+5] = HIBYTE(vsk);
|
||
// vi = vi + 6; }
|
||
|
||
/* USER CODE END SysTick_IRQn 0 */
|
||
HAL_IncTick();
|
||
/* USER CODE BEGIN SysTick_IRQn 1 */
|
||
|
||
/* USER CODE END SysTick_IRQn 1 */
|
||
}
|
||
|
||
/******************************************************************************/
|
||
/* STM32F1xx Peripheral Interrupt Handlers */
|
||
/* Add here the Interrupt Handlers for the used peripherals. */
|
||
/* For the available peripheral interrupt handler names, */
|
||
/* please refer to the startup file (startup_stm32f1xx.s). */
|
||
/******************************************************************************/
|
||
|
||
/**
|
||
* @brief This function handles EXTI line0 interrupt.
|
||
*/
|
||
void EXTI0_IRQHandler(void)
|
||
{
|
||
/* USER CODE BEGIN EXTI0_IRQn 0 */
|
||
if (avto == 1)
|
||
{
|
||
if (rejim_number < rejim[0])
|
||
rejim_number++;
|
||
else
|
||
rejim_number = 1;
|
||
|
||
SetServo(0, rejim[1 + (rejim_number - 1) * 6]);
|
||
SetServo(1, rejim[2 + (rejim_number - 1) * 6]);
|
||
SetServo(2, rejim[3 + (rejim_number - 1) * 6]);
|
||
Vz1 = rejim[4 + (rejim_number - 1) * 6];
|
||
Vz2 = rejim[5 + (rejim_number - 1) * 6];
|
||
TIM1->CCR1 = 0;
|
||
TIM1->CCR2 = (uint16_t)(40 * rejim[6 + (rejim_number - 1) * 6]);
|
||
vi = 0; // zapis grafika start
|
||
}
|
||
/* USER CODE END EXTI0_IRQn 0 */
|
||
HAL_GPIO_EXTI_IRQHandler(BALL_EXT_Pin);
|
||
/* USER CODE BEGIN EXTI0_IRQn 1 */
|
||
|
||
/* USER CODE END EXTI0_IRQn 1 */
|
||
}
|
||
|
||
/**
|
||
* @brief This function handles EXTI line1 interrupt.
|
||
*/
|
||
void EXTI1_IRQHandler(void)
|
||
{
|
||
/* USER CODE BEGIN EXTI1_IRQn 0 */
|
||
IR_handler();
|
||
/* USER CODE END EXTI1_IRQn 0 */
|
||
HAL_GPIO_EXTI_IRQHandler(IR_EXT_Pin);
|
||
/* USER CODE BEGIN EXTI1_IRQn 1 */
|
||
|
||
/* USER CODE END EXTI1_IRQn 1 */
|
||
}
|
||
|
||
/**
|
||
* @brief This function handles USB low priority or CAN RX0 interrupts.
|
||
*/
|
||
void USB_LP_CAN1_RX0_IRQHandler(void)
|
||
{
|
||
/* USER CODE BEGIN USB_LP_CAN1_RX0_IRQn 0 */
|
||
|
||
/* USER CODE END USB_LP_CAN1_RX0_IRQn 0 */
|
||
HAL_PCD_IRQHandler(&hpcd_USB_FS);
|
||
/* USER CODE BEGIN USB_LP_CAN1_RX0_IRQn 1 */
|
||
|
||
/* USER CODE END USB_LP_CAN1_RX0_IRQn 1 */
|
||
}
|
||
|
||
/**
|
||
* @brief This function handles TIM3 global interrupt.
|
||
*/
|
||
void TIM3_IRQHandler(void)
|
||
{
|
||
/* USER CODE BEGIN TIM3_IRQn 0 */
|
||
|
||
static uint32_t prev_capture1 = 0;
|
||
static uint32_t prev_capture2 = 0;
|
||
|
||
uint32_t current_capture1 = 0;
|
||
uint32_t current_capture2 = 0;
|
||
uint32_t elapsed_time1 = 0;
|
||
uint32_t elapsed_time2 = 0;
|
||
|
||
/* USER CODE BEGIN TIM3_IRQn 0 */
|
||
|
||
// Обработка переполнения таймера
|
||
if (TIM3->SR & TIM_SR_UIF)
|
||
{
|
||
// Сброс флага переполнения
|
||
TIM3->SR &= ~TIM_SR_UIF;
|
||
}
|
||
|
||
// Обработка захвата сигнала для канала 1
|
||
if (TIM3->SR & TIM_SR_CC1IF)
|
||
{
|
||
current_capture1 = TIM3->CCR1;
|
||
|
||
if (current_capture1 >= prev_capture1)
|
||
{
|
||
elapsed_time1 = current_capture1 - prev_capture1;
|
||
}
|
||
else
|
||
{
|
||
// Учёт переполнения таймера
|
||
elapsed_time1 = (65000 - prev_capture1) + current_capture1 + 1;
|
||
}
|
||
|
||
// Ограничение значения для сохранения в 16-битную переменную
|
||
if (elapsed_time1 > 65535)
|
||
{
|
||
timing1 = 65535; // Максимальное значение
|
||
}
|
||
else
|
||
{
|
||
timing1 = (uint16_t)elapsed_time1;
|
||
}
|
||
|
||
prev_capture1 = current_capture1;
|
||
TIM3->SR &= ~TIM_SR_CC1IF; // Сброс флага захвата сигнала
|
||
update1 = 0;
|
||
}
|
||
|
||
// Обработка захвата сигнала для канала 2
|
||
if (TIM3->SR & TIM_SR_CC2IF)
|
||
{
|
||
current_capture2 = TIM3->CCR2;
|
||
|
||
if (current_capture2 >= prev_capture2)
|
||
{
|
||
elapsed_time2 = current_capture2 - prev_capture2;
|
||
}
|
||
else
|
||
{
|
||
// Учёт переполнения таймера
|
||
elapsed_time2 = (65000 - prev_capture2) + current_capture2 + 1;
|
||
}
|
||
|
||
// Ограничение значения для сохранения в 16-битную переменную
|
||
if (elapsed_time2 > 65535)
|
||
{
|
||
timing2 = 65535; // Максимальное значение
|
||
}
|
||
else
|
||
{
|
||
timing2 = (uint16_t)elapsed_time2;
|
||
}
|
||
|
||
prev_capture2 = current_capture2;
|
||
TIM3->SR &= ~TIM_SR_CC2IF; // Сброс флага захвата сигнала
|
||
update2 = 0;
|
||
}
|
||
|
||
/* USER CODE END TIM3_IRQn 0 */
|
||
HAL_TIM_IRQHandler(&htim3);
|
||
/* USER CODE BEGIN TIM3_IRQn 1 */
|
||
|
||
/* USER CODE END TIM3_IRQn 1 */
|
||
}
|
||
|
||
/**
|
||
* @brief This function handles I2C1 event interrupt.
|
||
*/
|
||
void I2C1_EV_IRQHandler(void)
|
||
{
|
||
/* USER CODE BEGIN I2C1_EV_IRQn 0 */
|
||
|
||
/* USER CODE END I2C1_EV_IRQn 0 */
|
||
HAL_I2C_EV_IRQHandler(&hi2c1);
|
||
/* USER CODE BEGIN I2C1_EV_IRQn 1 */
|
||
|
||
/* USER CODE END I2C1_EV_IRQn 1 */
|
||
}
|
||
|
||
/**
|
||
* @brief This function handles I2C1 error interrupt.
|
||
*/
|
||
void I2C1_ER_IRQHandler(void)
|
||
{
|
||
/* USER CODE BEGIN I2C1_ER_IRQn 0 */
|
||
|
||
/* USER CODE END I2C1_ER_IRQn 0 */
|
||
HAL_I2C_ER_IRQHandler(&hi2c1);
|
||
/* USER CODE BEGIN I2C1_ER_IRQn 1 */
|
||
|
||
/* USER CODE END I2C1_ER_IRQn 1 */
|
||
}
|
||
|
||
/**
|
||
* @brief This function handles USART3 global interrupt.
|
||
*/
|
||
void USART3_IRQHandler(void)
|
||
{
|
||
/* USER CODE BEGIN USART3_IRQn 0 */
|
||
CDC_Transmit_FS(uart_rx_buffer, UART_BUFFER_SIZE);
|
||
HAL_UART_Receive_IT(&huart3, uart_rx_buffer, UART_BUFFER_SIZE);
|
||
|
||
/* USER CODE END USART3_IRQn 0 */
|
||
HAL_UART_IRQHandler(&huart3);
|
||
/* USER CODE BEGIN USART3_IRQn 1 */
|
||
|
||
/* USER CODE END USART3_IRQn 1 */
|
||
}
|
||
|
||
/* USER CODE BEGIN 1 */
|
||
|
||
/* USER CODE END 1 */
|