btl143/BC2D-POS-firmware
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: 增加速度环

Browse Source
This commit is contained in:
bmy
2024-04-26 16:13:07 +08:00
parent 05cab94699
commit 6bac98da0a
12 changed files with 303 additions and 136 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
3rd-part/PID-Library/pid.c
View File

@@ -36,10 +36,11 @@ void PID_Init(PID_TypeDef *uPID)
{
uPID->OutputSum = uPID->OutMin;
}
else { }
}
void PID(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, PIDPON_TypeDef POn, PIDCD_TypeDef ControllerDirection)
void PID(PID_TypeDef *uPID, const float *Input, float *Output, const float *Setpoint, float Kp, float Ki, float Kd, PIDPON_TypeDef POn, PIDCD_TypeDef ControllerDirection)
{
/* ~~~~~~~~~~ Set parameter ~~~~~~~~~~ */
uPID->MyOutput = Output;
@@ -56,7 +57,7 @@ void PID(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float
}
void PID2(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, PIDCD_TypeDef ControllerDirection)
void PID2(PID_TypeDef *uPID, const float *Input, float *Output, const float *Setpoint, float Kp, float Ki, float Kd, PIDCD_TypeDef ControllerDirection)
{
PID(uPID, Input, Output, Setpoint, Kp, Ki, Kd, _PID_P_ON_E, ControllerDirection);
}
@@ -65,10 +66,10 @@ void PID2(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float
uint8_t PID_Compute(PID_TypeDef *uPID)
{
float input=0;
float error=0;
float dInput=0;
float output=0;
float input;
float error;
float dInput;
float output;
/* ~~~~~~~~~~ Check PID mode ~~~~~~~~~~ */
if (!uPID->InAuto)

49
3rd-part/PID-Library/pid.h
View File

@@ -1,4 +1,4 @@
/*
/*
------------------------------------------------------------------------------
~ File : pid.h
~ Author : Majid Derhambakhsh
@@ -25,8 +25,6 @@
#include <stdint.h>
#include <string.h>
// #include "zf_common_headfile.h"
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Defines ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
/* ------------------------ Library ------------------------ */
#define _PID_LIBRARY_VERSION 1.0.0
@@ -37,36 +35,35 @@
#ifndef _FALSE
#define _FALSE 0
#define _FALSE 0
#endif
#ifndef _TRUE
#define _TRUE 1
#define _TRUE 1
#endif
/* ---------------------- By compiler ---------------------- */
#ifndef GetTime
/* ---------------------- By compiler ---------------------- */
/* ---------------------- By compiler ---------------------- */
#ifdef __CODEVISIONAVR__ /* Check compiler */
#ifdef __CODEVISIONAVR__ /* Check compiler */
#define GetTime() 0
#define GetTime() 0
/* ------------------------------------------------------------------ */
/* ------------------------------------------------------------------ */
#elif defined(__GNUC__) /* Check compiler */
#elif defined(__GNUC__) /* Check compiler */
#define GetTime() 0
#define GetTime() 0
/* ------------------------------------------------------------------ */
/* ------------------------------------------------------------------ */
#else
#endif /* __CODEVISIONAVR__ */
#else
#endif /* __CODEVISIONAVR__ */
/* ------------------------------------------------------------------ */
#endif
@@ -75,31 +72,28 @@
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Types ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
/* PID Mode */
typedef enum
{
typedef enum {
_PID_MODE_MANUAL = 0,
_PID_MODE_AUTOMATIC = 1
}PIDMode_TypeDef;
} PIDMode_TypeDef;
/* PID P On x */
typedef enum
{
typedef enum {
_PID_P_ON_M = 0, /* Proportional on Measurement */
_PID_P_ON_E = 1
}PIDPON_TypeDef;
} PIDPON_TypeDef;
/* PID Control direction */
typedef enum
{
typedef enum {
_PID_CD_DIRECT = 0,
_PID_CD_REVERSE = 1
}PIDCD_TypeDef;
} PIDCD_TypeDef;
/* PID Structure */
typedef struct
@@ -132,7 +126,7 @@ typedef struct
float OutMin;
float OutMax;
}PID_TypeDef;
} PID_TypeDef;
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Variables ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Enum ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
@@ -141,8 +135,9 @@ typedef struct
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Prototype ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
/* :::::::::::::: Init ::::::::::::: */
void PID_Init(PID_TypeDef *uPID);
void PID(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, PIDPON_TypeDef POn, PIDCD_TypeDef ControllerDirection);
void PID2(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, PIDCD_TypeDef ControllerDirection);
void PID(PID_TypeDef *uPID, const float *Input, float *Output, const float *Setpoint, float Kp, float Ki, float Kd, PIDPON_TypeDef POn, PIDCD_TypeDef ControllerDirection);
void PID2(PID_TypeDef *uPID, const float *Input, float *Output, const float *Setpoint, float Kp, float Ki, float Kd, PIDCD_TypeDef ControllerDirection);
/* ::::::::::: Computing ::::::::::: */
uint8_t PID_Compute(PID_TypeDef *uPID);

2
BC2D.ATWP
View File

@@ -81,7 +81,7 @@
<FLASH_IRQHandler>0;0;0</FLASH_IRQHandler>
<CRM_IRQHandler>0;0;0</CRM_IRQHandler>
<USBFS_H_CAN1_TX_IRQHandler>0;0;0</USBFS_H_CAN1_TX_IRQHandler>
<USBFS_L_CAN1_RX0_IRQHandler>0;0;0</USBFS_L_CAN1_RX0_IRQHandler>
<USBFS_L_CAN1_RX0_IRQHandler>1;1;0</USBFS_L_CAN1_RX0_IRQHandler>
<CAN1_RX1_IRQHandler>0;0;0</CAN1_RX1_IRQHandler>
<CAN1_SE_IRQHandler>0;0;0</CAN1_SE_IRQHandler>
<TMR1_BRK_TMR9_IRQHandler>0;0;0</TMR1_BRK_TMR9_IRQHandler>

3
BC2D.code-workspace
View File

@@ -30,7 +30,8 @@
"titleBar.activeBackground": "#1D18AE",
"titleBar.activeForeground": "#FAFAFE"
},
"EIDE.OpenOCD.ExePath": "D:/Program Files (x86)/at32_OpenOCD_V2.0.2/bin/openocd.exe"
"EIDE.OpenOCD.ExePath": "D:/Program Files (x86)/at32_OpenOCD_V2.0.2/bin/openocd.exe",
"cortex-debug.variableUseNaturalFormat": true
},
"extensions": {
}

19
app/by_can.c
View File

@@ -2,8 +2,11 @@
#include <string.h>
#include <assert.h>
#include "dwt_delay.h"
#include "by_utils.h"
#include "by_debug.h"
uint8_t by_can_send_stdd(uint32_t id, const uint8_t *data, uint8_t len)
by_error_status by_can_send_stdd(uint32_t id, const uint8_t *data, uint8_t len, uint16_t timeout)
{
assert(id < 0x7FF);
@@ -23,9 +26,15 @@ uint8_t by_can_send_stdd(uint32_t id, const uint8_t *data, uint8_t len)
transmit_mailbox = can_message_transmit(CAN1, &tx_message_struct); /* 将以上待发送报文写入发送邮箱并请求发送 */
/* 等待该邮箱发送成功—对应邮箱发送成功标志置起 */
while (can_transmit_status_get(CAN1, (can_tx_mailbox_num_type)transmit_mailbox) !=
CAN_TX_STATUS_SUCCESSFUL)
;
while (can_transmit_status_get(CAN1, (can_tx_mailbox_num_type)transmit_mailbox) != CAN_TX_STATUS_SUCCESSFUL) {
// LOGD("CAN#SEND: timeout=%d", timeout);
if (0 == timeout--) {
LOGW("CAN#TIMEOUT: ID=0x%x", id);
return 0;
return BY_ERROR;
}
DWT_Delay(10);
}
return BY_SUCCESS;
}

3
app/by_can.h
View File

@@ -2,7 +2,8 @@
#define _BY_CAN_H__
#include "at32f413.h"
#include "by_utils.h"
uint8_t by_can_send_stdd(uint32_t id, const uint8_t *data, uint8_t len);
by_error_status by_can_send_stdd(uint32_t id, const uint8_t *data, uint8_t len, uint16_t timeout);
#endif

104
app/by_motion.c
View File

@@ -1,15 +1,20 @@
#include "by_motion.h"
#include "pid.h"
#include "dwt_delay.h"
#include "by_utils.h"
#include "by_debug.h"
#define DRV_ENABLE() gpio_bits_set(GPIOB, GPIO_PINS_15)
#define DRV_DISABLE() gpio_bits_reset(GPIOB, GPIO_PINS_15)
int16_t speed_m1;
int16_t speed_m2;
by_motor_param param_m1;
by_motor_param param_m2;
PID_TypeDef pid_m1;
PID_TypeDef pid_m2;
uint8_t motion_enable_flag;
void by_motion_pwm_m1(int32_t pwm_duty)
void by_motion_set_pwm_m1(int32_t pwm_duty)
{
pwm_duty = clip_s32(pwm_duty, -449, 449); // 不可以拉满哦
pwm_duty += 499;
@@ -18,7 +23,7 @@ void by_motion_pwm_m1(int32_t pwm_duty)
tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, pwm_duty);
}
void by_motion_pwm_m2(int32_t pwm_duty)
void by_motion_set_pwm_m2(int32_t pwm_duty)
{
pwm_duty = clip_s32(pwm_duty, -449, 449); // 不可以拉满哦
pwm_duty += 499;
@@ -29,21 +34,100 @@ void by_motion_pwm_m2(int32_t pwm_duty)
int16_t by_motion_get_speed_m1(void)
{
speed_m1 = (int16_t)tmr_counter_value_get(TMR2);
#define alpha (0.1f)
static float last_speed = 0.0f;
param_m1.real_speed = alpha * last_speed + (1.0f - alpha) * (float)(-1 * (int16_t)tmr_counter_value_get(TMR2));
last_speed = param_m1.real_speed;
tmr_counter_value_set(TMR2, 0);
return speed_m1;
return (int16_t)param_m1.real_speed;
#undef alpha
}
int16_t by_motion_get_speed_m2(void)
{
speed_m2 = (int16_t)tmr_counter_value_get(TMR3);
#define alpha (0.1f)
static float last_speed = 0.0f;
param_m2.real_speed = alpha * last_speed + (1.0f - alpha) * (float)(-1 * (int16_t)tmr_counter_value_get(TMR3));
last_speed = param_m2.real_speed;
tmr_counter_value_set(TMR3, 0);
return speed_m2;
return (int16_t)param_m2.real_speed;
#undef alpha
}
void by_motion_set_speed_m1(int16_t speed)
{
param_m1.target_speed = speed;
}
void by_motion_set_speed_m2(int16_t speed)
{
param_m2.target_speed = speed;
}
void by_motion_init(void)
{
motion_enable_flag = 0;
by_motion_set_pwm_m1(0);
by_motion_set_pwm_m2(0);
/* set default parameters */
param_m1.Kp = BY_MOTION_DEFAULT_KP_M1;
param_m1.Ki = BY_MOTION_DEFAULT_KI_M1;
param_m1.Kd = BY_MOTION_DEFAULT_KD_M1;
param_m2.Kp = BY_MOTION_DEFAULT_KP_M2;
param_m2.Ki = BY_MOTION_DEFAULT_KI_M2;
param_m2.Kd = BY_MOTION_DEFAULT_KD_M2;
/* load parameters form ee */
// TODO
PID(&pid_m1, &param_m1.real_speed, &param_m1.out_pwm, &param_m1.target_speed, param_m1.Kp, param_m1.Ki, param_m1.Kd, _PID_P_ON_E, _PID_CD_DIRECT);
PID_SetMode(&pid_m1, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&pid_m1, 10);
PID_SetOutputLimits(&pid_m1, -400, 400);
PID(&pid_m2, &param_m2.real_speed, &param_m2.out_pwm, &param_m2.target_speed, param_m2.Kp, param_m2.Ki, param_m2.Kd, _PID_P_ON_E, _PID_CD_DIRECT);
PID_SetMode(&pid_m2, _PID_MODE_AUTOMATIC);
PID_SetSampleTime(&pid_m2, 10);
PID_SetOutputLimits(&pid_m2, -400, 400);
motion_enable_flag = 1;
DRV_ENABLE();
by_motion_pwm_m1(-125);
by_motion_pwm_m2(0);
}
void by_motion_run(void)
{
if (motion_enable_flag) {
by_motion_get_speed_m1();
by_motion_get_speed_m2();
PID_Compute(&pid_m1);
PID_Compute(&pid_m2);
by_motion_set_pwm_m1((int32_t)param_m1.out_pwm);
by_motion_set_pwm_m2((int32_t)param_m2.out_pwm);
}
}
void by_motion_can_handle(uint16_t stdd_id, const uint8_t *data, uint8_t len)
{
#define BC2D_MODEL2
if (0x01 == stdd_id) {
#if defined(BC2D_MODEL1)
int16_t speed_m1_temp = (int16_t)data[0] | ((int16_t)data[1] << 8);
int16_t speed_m2_temp = (int16_t)data[2] | ((int16_t)data[3] << 8);
by_motion_set_speed_m1(speed_m1_temp);
by_motion_set_speed_m2(speed_m2_temp);
#elif defined(BC2D_MODEL2)
int16_t speed_m1_temp = (int16_t)(data[4] | (data[5] << 8));
int16_t speed_m2_temp = (int16_t)(data[6] | (data[7] << 8));
by_motion_set_speed_m1(speed_m1_temp);
by_motion_set_speed_m2(speed_m2_temp);
#endif
}
LOGD("m1:%f,%f,%f", param_m1.real_speed, param_m1.target_speed, param_m1.out_pwm);
LOGD("m2:%f,%f,%f", param_m2.real_speed, param_m2.target_speed, param_m2.out_pwm);
}

26
app/by_motion.h
View File

@@ -3,11 +3,33 @@
#include "at32f413.h"
typedef struct by_motor_param {
float Kp;
float Ki;
float Kd;
float real_speed;
float target_speed;
float out_pwm;
} by_motor_param;
#define BY_MOTION_DEFAULT_KP_M1 (5.0f)
#define BY_MOTION_DEFAULT_KI_M1 (100.0f)
#define BY_MOTION_DEFAULT_KD_M1 (0.0f)
#define BY_MOTION_DEFAULT_KP_M2 (5.0f)
#define BY_MOTION_DEFAULT_KI_M2 (100.0f)
#define BY_MOTION_DEFAULT_KD_M2 (0.0f)
extern void by_motion_init(void);
extern void by_motion_run(void);
extern void by_motion_set_pwm_m1(int32_t pwm_duty);
extern void by_motion_set_pwm_m2(int32_t pwm_duty);
extern int16_t by_motion_get_speed_m1(void);
extern int16_t by_motion_get_speed_m2(void);
extern void by_motion_set_speed_m1(int16_t speed);
extern void by_motion_set_speed_m2(int16_t speed);
extern void by_motion_can_handle(uint16_t stdd_id, const uint8_t *data, uint8_t len);
extern int16_t speed_m1;
extern int16_t speed_m2;
extern by_motor_param param_m1;
extern by_motor_param param_m2;
#endif

15
app/by_utils.h
View File

@@ -3,6 +3,21 @@
#include "at32f413.h"
typedef enum {
BY_ERROR = 0,
BY_SUCCESS = !BY_ERROR
} by_error_status;
typedef enum {
T_U8 = 0,
T_U16,
T_U32,
T_S8,
T_S16,
T_S32,
T_F32
} by_data_type;
int32_t clip_s32(int32_t x, int32_t low, int32_t up);
#endif

1
project/inc/at32f413_int.h
View File

@@ -65,6 +65,7 @@ void SVC_Handler(void);
void DebugMon_Handler(void);
void PendSV_Handler(void);
void USBFS_L_CAN1_RX0_IRQHandler(void);
void TMR4_GLOBAL_IRQHandler(void);
/* add user code begin exported functions */

38
project/src/at32f413_int.c
View File

@@ -29,6 +29,7 @@
/* private includes ----------------------------------------------------------*/
/* add user code begin private includes */
#include "by_debug.h"
#include "by_motion.h"
/* add user code end private includes */
@@ -94,7 +95,8 @@ void HardFault_Handler(void)
/* add user code end HardFault_IRQ 0 */
/* go to infinite loop when hard fault exception occurs */
while (1) {
while (1)
{
/* add user code begin W1_HardFault_IRQ 0 */
/* add user code end W1_HardFault_IRQ 0 */
@@ -112,7 +114,8 @@ void MemManage_Handler(void)
/* add user code end MemoryManagement_IRQ 0 */
/* go to infinite loop when memory manage exception occurs */
while (1) {
while (1)
{
/* add user code begin W1_MemoryManagement_IRQ 0 */
/* add user code end W1_MemoryManagement_IRQ 0 */
@@ -130,7 +133,8 @@ void BusFault_Handler(void)
/* add user code end BusFault_IRQ 0 */
/* go to infinite loop when bus fault exception occurs */
while (1) {
while (1)
{
/* add user code begin W1_BusFault_IRQ 0 */
/* add user code end W1_BusFault_IRQ 0 */
@@ -148,7 +152,8 @@ void UsageFault_Handler(void)
/* add user code end UsageFault_IRQ 0 */
/* go to infinite loop when usage fault exception occurs */
while (1) {
while (1)
{
/* add user code begin W1_UsageFault_IRQ 0 */
/* add user code end W1_UsageFault_IRQ 0 */
@@ -200,6 +205,28 @@ void PendSV_Handler(void)
/* add user code end PendSV_IRQ 1 */
}
/**
* @brief this function handles USB Low Priority or CAN1 RX0 handler.
* @param none
* @retval none
*/
void USBFS_L_CAN1_RX0_IRQHandler(void)
{
/* add user code begin USBFS_L_CAN1_RX0_IRQ 0 */
if (SET == can_flag_get(CAN1, CAN_RF0MN_FLAG)) {
can_rx_message_type can_rx_message;
can_message_receive(CAN1, CAN_RX_FIFO0, &can_rx_message);
by_motion_can_handle((uint16_t)can_rx_message.standard_id, can_rx_message.data, can_rx_message.dlc);
can_flag_clear(CAN1, CAN_RF0MN_FLAG);
}
/* add user code end USBFS_L_CAN1_RX0_IRQ 0 */
/* add user code begin USBFS_L_CAN1_RX0_IRQ 1 */
/* add user code end USBFS_L_CAN1_RX0_IRQ 1 */
}
/**
* @brief this function handles TMR4 handler.
* @param none
@@ -209,8 +236,7 @@ void TMR4_GLOBAL_IRQHandler(void)
{
/* add user code begin TMR4_GLOBAL_IRQ 0 */
if (SET == tmr_interrupt_flag_get(TMR4, TMR_OVF_FLAG)) {
by_motion_get_speed_m1();
by_motion_get_speed_m2();
by_motion_run();
tmr_flag_clear(TMR4, TMR_OVF_FLAG);
}
/* add user code end TMR4_GLOBAL_IRQ 0 */

14
project/src/at32f413_wk_config.c
View File

@@ -204,6 +204,7 @@ void wk_nvic_config(void)
{
nvic_priority_group_config(NVIC_PRIORITY_GROUP_4);
nvic_irq_enable(USBFS_L_CAN1_RX0_IRQn, 1, 0);
nvic_irq_enable(TMR4_GLOBAL_IRQn, 0, 0);
}
@@ -660,8 +661,19 @@ void wk_can1_init(void)
can_filter_init(CAN1, &can_filter_init_struct);
/* add user code begin can1_init 2 */
/**
* Users need to configure CAN1 interrupt functions according to the actual application.
* 1. Call the below function to enable the corresponding CAN1 interrupt.
* --can_interrupt_enable(...)
* 2. Add the user's interrupt handler code into the below function in the at32f413_int.c file.
* --void USBFS_L_CAN1_RX0_IRQHandler(void)
*/
/*can1 rx0 interrupt config--------------------------------------------------------*/
//can_interrupt_enable(CAN1, CAN_RF0MIEN_INT, TRUE);
/* add user code begin can1_init 2 */
can_interrupt_enable(CAN1, CAN_RF0MIEN_INT, TRUE);
/* add user code end can1_init 2 */
}