mirror of
https://github.com/DashyFox/StackSport.git
synced 2025-06-28 05:09:32 +00:00
IR_CMD_Handler
This commit is contained in:
@ -1,20 +1,20 @@
|
||||
/* 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.
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
******************************************************************************
|
||||
* @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 ------------------------------------------------------------------*/
|
||||
@ -27,6 +27,8 @@
|
||||
|
||||
#include "usb_device.h"
|
||||
#include "usbd_cdc_if.h"
|
||||
|
||||
#include "GlobalDefines.h"
|
||||
/* USER CODE END Includes */
|
||||
|
||||
/* Private typedef -----------------------------------------------------------*/
|
||||
@ -46,43 +48,39 @@
|
||||
|
||||
/* Private variables ---------------------------------------------------------*/
|
||||
/* USER CODE BEGIN PV */
|
||||
extern volatile uint32_t millisCounter;
|
||||
|
||||
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; //<2F><><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 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; // <EFBFBD><EFBFBD><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; //<2F><><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]; //<2F><><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>
|
||||
extern uint16_t vi; // <EFBFBD><EFBFBD><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]; // <EFBFBD><EFBFBD><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; //<2F><><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 vt1 = 0; // <EFBFBD><EFBFBD><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}; //<2F><><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
|
||||
uint16_t vs1[4] = {0, 0, 0, 0}; // <EFBFBD><EFBFBD><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; //<2F><><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 vsk1 = 0; // <EFBFBD><EFBFBD><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; //<2F><><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; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
uint16_t Vzad2 = 0; //<2F><><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; //<2F><><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
|
||||
uint16_t Vzad1 = 0; // <EFBFBD><EFBFBD><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; // <EFBFBD><EFBFBD><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; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
extern uint16_t timing2; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
extern uint8_t velosety[600]; //<2F><><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><>
|
||||
extern uint16_t timing1; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
extern uint16_t timing2; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
extern uint8_t velosety[600]; // <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><>
|
||||
|
||||
extern uint8_t Vz1;
|
||||
extern uint8_t Vz2;
|
||||
@ -99,49 +97,84 @@ int32_t computePID2(uint16_t input, uint16_t setpoint);
|
||||
/* 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>
|
||||
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>
|
||||
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;
|
||||
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;}
|
||||
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;}
|
||||
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;} //
|
||||
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;
|
||||
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>
|
||||
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;}
|
||||
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;}
|
||||
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;} //
|
||||
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;
|
||||
return Rout;
|
||||
}
|
||||
/* USER CODE END 0 */
|
||||
|
||||
@ -149,6 +182,7 @@ int32_t computePID2(uint16_t input, uint16_t setpoint)
|
||||
extern PCD_HandleTypeDef hpcd_USB_FS;
|
||||
extern I2C_HandleTypeDef hi2c1;
|
||||
extern TIM_HandleTypeDef htim3;
|
||||
extern UART_HandleTypeDef huart3;
|
||||
/* USER CODE BEGIN EV */
|
||||
|
||||
/* USER CODE END EV */
|
||||
@ -165,7 +199,7 @@ void NMI_Handler(void)
|
||||
|
||||
/* USER CODE END NonMaskableInt_IRQn 0 */
|
||||
/* USER CODE BEGIN NonMaskableInt_IRQn 1 */
|
||||
while (1)
|
||||
while (1)
|
||||
{
|
||||
}
|
||||
/* USER CODE END NonMaskableInt_IRQn 1 */
|
||||
@ -277,95 +311,166 @@ void SysTick_Handler(void)
|
||||
{
|
||||
/* USER CODE BEGIN SysTick_IRQn 0 */
|
||||
|
||||
//<2F><><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) { //<2F><><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>
|
||||
//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
vt1 = 0;
|
||||
for (uint8_t i = 1; i <= v1[0]; i++) { vt1 += v1[i]; }
|
||||
vt1 = vt1/v1[0];
|
||||
//<2F><><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>
|
||||
//<2F><><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> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
// <EFBFBD><EFBFBD><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> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><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> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>, <20><> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><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>
|
||||
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 2
|
||||
if (v2[0] > 0) { //<2F><><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>
|
||||
//<2F><><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];
|
||||
//<2F><><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>
|
||||
//<2F><><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><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD> 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;
|
||||
|
||||
//<2F><><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]++; }
|
||||
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]++;
|
||||
}
|
||||
|
||||
vsk1 = (vs1[1]+vs1[2]+vs1[3])/3;
|
||||
vsk2 = (vs2[1] + vs2[2] + vs2[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]++; }
|
||||
// <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;
|
||||
|
||||
vsk2 = (vs2[1]+vs2[2]+vs2[3])/3;
|
||||
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> <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;
|
||||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
Vupr1 = (uint16_t)computePID1(vsk1, Vzad1);
|
||||
Vupr2 = (uint16_t)computePID2(vsk2, Vzad2);
|
||||
|
||||
if (vsk2 != 0) {vsk2 = 6000000/vsk2;} else { vsk2 = 0;}
|
||||
if (vsk2 > 10000) vsk2 = 10000;
|
||||
if (vsk2 < 150) vsk2 = 0;
|
||||
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><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 0 - 200 <20> -8000 ... 8000 <20><>./<2F><EFBFBD><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><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <EFBFBD><EFBFBD>
|
||||
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;
|
||||
}
|
||||
|
||||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
Vupr1 = (uint16_t)computePID1(vsk1, Vzad1);
|
||||
Vupr2 = (uint16_t)computePID2(vsk2, Vzad2);
|
||||
// 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; }
|
||||
|
||||
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; }
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
millisCounter+=10; // костыльная примерная коррекция
|
||||
/* USER CODE END SysTick_IRQn 0 */
|
||||
HAL_IncTick();
|
||||
/* USER CODE BEGIN SysTick_IRQn 1 */
|
||||
@ -386,17 +491,22 @@ if (vi < 600) { velosety[vi] = LOBYTE(timing1);
|
||||
void EXTI0_IRQHandler(void)
|
||||
{
|
||||
/* USER CODE BEGIN EXTI0_IRQn 0 */
|
||||
if (avto == 1) { if (rejim_number < rejim[0]) rejim_number++; else rejim_number = 1;
|
||||
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
|
||||
|
||||
}
|
||||
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 */
|
||||
@ -410,7 +520,7 @@ void EXTI0_IRQHandler(void)
|
||||
void EXTI1_IRQHandler(void)
|
||||
{
|
||||
/* USER CODE BEGIN EXTI1_IRQn 0 */
|
||||
IR_handler();
|
||||
IR_handler();
|
||||
/* USER CODE END EXTI1_IRQn 0 */
|
||||
HAL_GPIO_EXTI_IRQHandler(IR_EXT_Pin);
|
||||
/* USER CODE BEGIN EXTI1_IRQn 1 */
|
||||
@ -439,70 +549,82 @@ void TIM3_IRQHandler(void)
|
||||
{
|
||||
/* USER CODE BEGIN TIM3_IRQn 0 */
|
||||
|
||||
static uint32_t prev_capture1 = 0;
|
||||
static uint32_t prev_capture2 = 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;
|
||||
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 */
|
||||
/* USER CODE BEGIN TIM3_IRQn 0 */
|
||||
|
||||
// Обработка переполнения таймера
|
||||
if (TIM3->SR & TIM_SR_UIF)
|
||||
{
|
||||
// Сброс флага переполнения
|
||||
TIM3->SR &= ~TIM_SR_UIF;
|
||||
}
|
||||
// Обработка переполнения таймера
|
||||
if (TIM3->SR & TIM_SR_UIF)
|
||||
{
|
||||
// Сброс флага переполнения
|
||||
TIM3->SR &= ~TIM_SR_UIF;
|
||||
}
|
||||
|
||||
// Обработка захвата сигнала для канала 1
|
||||
if (TIM3->SR & TIM_SR_CC1IF)
|
||||
{
|
||||
current_capture1 = TIM3->CCR1;
|
||||
// Обработка захвата сигнала для канала 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;
|
||||
}
|
||||
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;
|
||||
}
|
||||
// Ограничение значения для сохранения в 16-битную переменную
|
||||
if (elapsed_time1 > 65535)
|
||||
{
|
||||
timing1 = 65535; // Максимальное значение
|
||||
}
|
||||
else
|
||||
{
|
||||
timing1 = (uint16_t)elapsed_time1;
|
||||
}
|
||||
|
||||
prev_capture1 = current_capture1;
|
||||
TIM3->SR &= ~TIM_SR_CC1IF; // Сброс флага захвата сигнала
|
||||
update1 = 0;
|
||||
}
|
||||
prev_capture1 = current_capture1;
|
||||
TIM3->SR &= ~TIM_SR_CC1IF; // Сброс флага захвата сигнала
|
||||
update1 = 0;
|
||||
}
|
||||
|
||||
// Обработка захвата сигнала для канала 2
|
||||
if (TIM3->SR & TIM_SR_CC2IF)
|
||||
{
|
||||
current_capture2 = TIM3->CCR2;
|
||||
// Обработка захвата сигнала для канала 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;
|
||||
}
|
||||
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;
|
||||
}
|
||||
// Ограничение значения для сохранения в 16-битную переменную
|
||||
if (elapsed_time2 > 65535)
|
||||
{
|
||||
timing2 = 65535; // Максимальное значение
|
||||
}
|
||||
else
|
||||
{
|
||||
timing2 = (uint16_t)elapsed_time2;
|
||||
}
|
||||
|
||||
prev_capture2 = current_capture2;
|
||||
TIM3->SR &= ~TIM_SR_CC2IF; // Сброс флага захвата сигнала
|
||||
update2 = 0;
|
||||
}
|
||||
prev_capture2 = current_capture2;
|
||||
TIM3->SR &= ~TIM_SR_CC2IF; // Сброс флага захвата сигнала
|
||||
update2 = 0;
|
||||
}
|
||||
|
||||
/* USER CODE END TIM3_IRQn 0 */
|
||||
HAL_TIM_IRQHandler(&htim3);
|
||||
@ -539,6 +661,22 @@ void I2C1_ER_IRQHandler(void)
|
||||
/* 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 */
|
||||
|
||||
Reference in New Issue
Block a user