From f0cbc9f1d3e719ea09a37a13e3678791911dba5b Mon Sep 17 00:00:00 2001 From: CaoWangrenbo Date: Tue, 16 Apr 2024 18:17:30 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E5=AE=8C=E6=88=90=E4=BA=92=E8=A1=A5pwm?= =?UTF-8?q?=E8=BE=93=E5=87=BA=E6=8E=A5=E5=8F=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clang-format | 4 +- .eide/eide.json | 7 +- 3rd-part/PID-Library/LICENSE | 21 +++ 3rd-part/PID-Library/README.md | 164 +++++++++++++++++ 3rd-part/PID-Library/pid.c | 292 +++++++++++++++++++++++++++++++ 3rd-part/PID-Library/pid.h | 175 ++++++++++++++++++ BC2D.ATWP | 27 +-- BC2D.code-workspace | 3 +- app/by_motion.c | 33 ++++ app/by_motion.h | 8 + app/by_utils.c | 7 + app/by_utils.h | 8 + project/inc/at32f413_conf.h | 2 +- project/inc/at32f413_wk_config.h | 3 - project/src/at32f413_wk_config.c | 74 +++----- project/src/main.c | 19 +- 16 files changed, 765 insertions(+), 82 deletions(-) create mode 100644 3rd-part/PID-Library/LICENSE create mode 100644 3rd-part/PID-Library/README.md create mode 100644 3rd-part/PID-Library/pid.c create mode 100644 3rd-part/PID-Library/pid.h create mode 100644 app/by_motion.c create mode 100644 app/by_motion.h create mode 100644 app/by_utils.c create mode 100644 app/by_utils.h diff --git a/.clang-format b/.clang-format index ecc57c4..408ff24 100644 --- a/.clang-format +++ b/.clang-format @@ -7,8 +7,8 @@ Language: Cpp ################################### UseTab: Never -IndentWidth: 4 -TabWidth: 4 +IndentWidth: 2 +TabWidth: 2 ColumnLimit: 0 AccessModifierOffset: -4 NamespaceIndentation: All diff --git a/.eide/eide.json b/.eide/eide.json index b0c024a..1781bd1 100644 --- a/.eide/eide.json +++ b/.eide/eide.json @@ -4,7 +4,8 @@ "dependenceList": [], "srcDirs": [ ".eide/deps", - "3rd-part" + "3rd-part", + "app" ], "virtualFolder": { "name": "", @@ -126,7 +127,9 @@ ".cmsis/include", "project/MDK_V5/RTE/_BC2D", ".eide/deps", - "3rd-part/dwt_delay" + "3rd-part/dwt_delay", + "3rd-part/PID-Library", + "app" ], "libList": [], "sourceDirList": [], diff --git a/3rd-part/PID-Library/LICENSE b/3rd-part/PID-Library/LICENSE new file mode 100644 index 0000000..7515c7e --- /dev/null +++ b/3rd-part/PID-Library/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2021 Majid Derhambakhsh + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/3rd-part/PID-Library/README.md b/3rd-part/PID-Library/README.md new file mode 100644 index 0000000..317c992 --- /dev/null +++ b/3rd-part/PID-Library/README.md @@ -0,0 +1,164 @@ +![Banner](Banner.png) + +# PID-Library (C Version) +PID Controller library for ARM Cortex M (STM32) + +> #### Download Arduino Library : [Arduino-PID-Library](https://github.com/br3ttb/Arduino-PID-Library) + +## Release +- #### Version : 1.0.0 + +- #### Type : Embedded Software. + +- #### Support : + - ARM STM32 series + +- #### Program Language : C/C++ + +- #### Properties : + +- #### Changes : + +- #### Required Library/Driver : + + +## Overview +### Initialization and de-initialization functions: +```c++ +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); +``` + +### Operation functions: +```c++ +/* ::::::::::: Computing ::::::::::: */ +uint8_t PID_Compute(PID_TypeDef *uPID); + +/* ::::::::::: PID Mode :::::::::::: */ +void PID_SetMode(PID_TypeDef *uPID, PIDMode_TypeDef Mode); +PIDMode_TypeDef PID_GetMode(PID_TypeDef *uPID); + +/* :::::::::: PID Limits ::::::::::: */ +void PID_SetOutputLimits(PID_TypeDef *uPID, float Min, float Max); + +/* :::::::::: PID Tunings :::::::::: */ +void PID_SetTunings(PID_TypeDef *uPID, float Kp, float Ki, float Kd); +void PID_SetTunings2(PID_TypeDef *uPID, float Kp, float Ki, float Kd, PIDPON_TypeDef POn); + +/* ::::::::: PID Direction ::::::::: */ +void PID_SetControllerDirection(PID_TypeDef *uPID, PIDCD_TypeDef Direction); +PIDCD_TypeDef PID_GetDirection(PID_TypeDef *uPID); + +/* ::::::::: PID Sampling :::::::::: */ +void PID_SetSampleTime(PID_TypeDef *uPID, int32_t NewSampleTime); + +/* ::::::: Get Tunings Param ::::::: */ +float PID_GetKp(PID_TypeDef *uPID); +float PID_GetKi(PID_TypeDef *uPID); +float PID_GetKd(PID_TypeDef *uPID); +``` + +### Macros: +```diff +non +``` + +## Guide + +#### This library can be used as follows: +#### 1. Add pid.h header +#### 2. Create PID struct and initialize it, for example: +* Initializer: + ```c++ + PID(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, PIDPON_TypeDef POn, PIDCD_TypeDef ControllerDirection); + ``` +* Parameters: + * uPID : Pointer to pid struct + * Input : The variable we're trying to control (float) + * Output : The variable that will be adjusted by the pid (float) + * Setpoint : The value we want to Input to maintain (float) + * Kp,Ki,Kd : Tuning Parameters. these affect how the pid will change the output (float>=0) + * POn : Either P_ON_E (Default) or P_ON_M. Allows Proportional on Measurement to be specified. + * ControllerDirection : Either DIRECT or REVERSE. determines which direction the output will move when faced with a given error. DIRECT is most common + + +* Example: + ```c++ + PID_TypeDef TPID; + + float Temp, PIDOut, TempSetpoint; + + PID(&TPID, &Temp, &PIDOut, &TempSetpoint, 2, 5, 1, _PID_P_ON_E, _PID_CD_DIRECT); + ``` +#### 3. Set 'mode', 'sample time' and 'output limit', for example: +* Functions: + ```c++ + void PID_SetMode(PID_TypeDef *uPID, PIDMode_TypeDef Mode); + void PID_SetOutputLimits(PID_TypeDef *uPID, float Min, float Max); + void PID_SetSampleTime(PID_TypeDef *uPID, int32_t NewSampleTime); + ``` +* Parameters: + * uPID : Pointer to pid struct + * Mode : _PID_MODE_AUTOMATIC or _PID_MODE_MANUAL + * Min : Low end of the range. must be < max (float) + * Max : High end of the range. must be > min (float) + * NewSampleTime : How often, in milliseconds, the PID will be evaluated. (int>0) + +* Example: + ```c++ + PID_SetMode(&TPID, _PID_MODE_AUTOMATIC); + PID_SetSampleTime(&TPID, 500); + PID_SetOutputLimits(&TPID, 1, 100); + ``` + +#### 4. Using Compute function, for example: + +```c++ +PID_Compute(&TPID); +``` + +## Examples + +#### Example 1: PID Compute for temperature +```c++ +#include "main.h" +#include "pid.h" + +PID_TypeDef TPID; + +char OutBuf[50]; +float Temp, PIDOut, TempSetpoint; + +int main(void) +{ + + HW_Init(); + + PID(&TPID, &Temp, &PIDOut, &TempSetpoint, 2, 5, 1, _PID_P_ON_E, _PID_CD_DIRECT); + + PID_SetMode(&TPID, _PID_MODE_AUTOMATIC); + PID_SetSampleTime(&TPID, 500); + PID_SetOutputLimits(&TPID, 1, 100); + + while (1) + { + + Temp = GetTemp(); + PID_Compute(&TPID); + + sprintf(OutBuf, "Temp%3.2f : %u\n", Temp, (uint16_t)PIDOut); + UART_Transmit((uint8_t *)OutBuf, strlen(OutBuf)); + + Delay_ms(500); + + } +} + +``` + +## Tests performed: +- [x] Run on STM32 Fx cores + +## Developers: +- ### Majid Derhambakhsh + diff --git a/3rd-part/PID-Library/pid.c b/3rd-part/PID-Library/pid.c new file mode 100644 index 0000000..d65326c --- /dev/null +++ b/3rd-part/PID-Library/pid.c @@ -0,0 +1,292 @@ + /* +------------------------------------------------------------------------------ +~ File : pid.c +~ Author : Majid Derhambakhsh +~ Version: V1.0.0 +~ Created: 02/11/2021 03:43:00 AM +~ Brief : +~ Support: + E-Mail : Majid.do16@gmail.com (subject : Embedded Library Support) + + Github : https://github.com/Majid-Derhambakhsh +------------------------------------------------------------------------------ +~ Description: + +~ Attention : + +~ Changes : +------------------------------------------------------------------------------ +*/ + +#include "pid.h" + +/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Functions ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ +/* ~~~~~~~~~~~~~~~~~ Initialize ~~~~~~~~~~~~~~~~ */ +void PID_Init(PID_TypeDef *uPID) +{ + /* ~~~~~~~~~~ Set parameter ~~~~~~~~~~ */ + uPID->OutputSum = *uPID->MyOutput; + uPID->LastInput = *uPID->MyInput; + + if (uPID->OutputSum > uPID->OutMax) + { + uPID->OutputSum = uPID->OutMax; + } + else if (uPID->OutputSum < uPID->OutMin) + { + uPID->OutputSum = uPID->OutMin; + } + + +} +void PID(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, PIDPON_TypeDef POn, PIDCD_TypeDef ControllerDirection) +{ + /* ~~~~~~~~~~ Set parameter ~~~~~~~~~~ */ + uPID->MyOutput = Output; + uPID->MyInput = Input; + uPID->MySetpoint = Setpoint; + uPID->InAuto = (PIDMode_TypeDef)_FALSE; + + PID_SetOutputLimits(uPID, 0, _PID_8BIT_PWM_MAX); + + uPID->SampleTime = _PID_SAMPLE_TIME_MS_DEF; /* default Controller Sample Time is 0.1 seconds */ + + PID_SetControllerDirection(uPID, ControllerDirection); + PID_SetTunings2(uPID, Kp, Ki, Kd, POn); + +} + +void PID2(PID_TypeDef *uPID, float *Input, float *Output, float *Setpoint, float Kp, float Ki, float Kd, PIDCD_TypeDef ControllerDirection) +{ + PID(uPID, Input, Output, Setpoint, Kp, Ki, Kd, _PID_P_ON_E, ControllerDirection); +} + +/* ~~~~~~~~~~~~~~~~~ Computing ~~~~~~~~~~~~~~~~~ */ +uint8_t PID_Compute(PID_TypeDef *uPID) +{ + + float input=0; + float error=0; + float dInput=0; + float output=0; + + /* ~~~~~~~~~~ Check PID mode ~~~~~~~~~~ */ + if (!uPID->InAuto) + { + return _FALSE; + } + + + /* ..... Compute all the working error variables ..... */ + input = *uPID->MyInput; + error = *uPID->MySetpoint - input; + dInput = (input - uPID->LastInput); + + uPID->OutputSum += (uPID->Ki * error); + + /* ..... Add Proportional on Measurement, if P_ON_M is specified ..... */ + if (!uPID->POnE) + { + uPID->OutputSum -= uPID->Kp * dInput; + } + + if (uPID->OutputSum > uPID->OutMax) + { + uPID->OutputSum = uPID->OutMax; + } + else if (uPID->OutputSum < uPID->OutMin) + { + uPID->OutputSum = uPID->OutMin; + } + else { } + + /* ..... Add Proportional on Error, if P_ON_E is specified ..... */ + if (uPID->POnE) + { + output = uPID->Kp * error; + } + else + { + output = 0; + } + + /* ..... Compute Rest of PID Output ..... */ + output += uPID->OutputSum - uPID->Kd * dInput; + + if (output > uPID->OutMax) + { + output = uPID->OutMax; + } + else if (output < uPID->OutMin) + { + output = uPID->OutMin; + } + else { } + + *uPID->MyOutput = output; + + /* ..... Remember some variables for next time ..... */ + uPID->LastInput = input; + + return _TRUE; +} + +/* ~~~~~~~~~~~~~~~~~ PID Mode ~~~~~~~~~~~~~~~~~~ */ +void PID_SetMode(PID_TypeDef *uPID, PIDMode_TypeDef Mode) +{ + + uint8_t newAuto = (Mode == _PID_MODE_AUTOMATIC); + + /* ~~~~~~~~~~ Initialize the PID ~~~~~~~~~~ */ + if (newAuto && !uPID->InAuto) + { + PID_Init(uPID); + } + + uPID->InAuto = (PIDMode_TypeDef)newAuto; + +} +PIDMode_TypeDef PID_GetMode(PID_TypeDef *uPID) +{ + return uPID->InAuto ? _PID_MODE_AUTOMATIC : _PID_MODE_MANUAL; +} + +/* ~~~~~~~~~~~~~~~~ PID Limits ~~~~~~~~~~~~~~~~~ */ +void PID_SetOutputLimits(PID_TypeDef *uPID, float Min, float Max) +{ + /* ~~~~~~~~~~ Check value ~~~~~~~~~~ */ + if (Min >= Max) + { + return; + } + + uPID->OutMin = Min; + uPID->OutMax = Max; + + /* ~~~~~~~~~~ Check PID Mode ~~~~~~~~~~ */ + if (uPID->InAuto) + { + + /* ..... Check out value ..... */ + if (*uPID->MyOutput > uPID->OutMax) + { + *uPID->MyOutput = uPID->OutMax; + } + else if (*uPID->MyOutput < uPID->OutMin) + { + *uPID->MyOutput = uPID->OutMin; + } + else { } + + /* ..... Check out value ..... */ + if (uPID->OutputSum > uPID->OutMax) + { + uPID->OutputSum = uPID->OutMax; + } + else if (uPID->OutputSum < uPID->OutMin) + { + uPID->OutputSum = uPID->OutMin; + } + else { } + + } + +} + +/* ~~~~~~~~~~~~~~~~ PID Tunings ~~~~~~~~~~~~~~~~ */ +void PID_SetTunings(PID_TypeDef *uPID, float Kp, float Ki, float Kd) +{ + PID_SetTunings2(uPID, Kp, Ki, Kd, uPID->POn); +} +void PID_SetTunings2(PID_TypeDef *uPID, float Kp, float Ki, float Kd, PIDPON_TypeDef POn) +{ + + float SampleTimeInSec; + + /* ~~~~~~~~~~ Check value ~~~~~~~~~~ */ + if (Kp < 0 || Ki < 0 || Kd < 0) + { + return; + } + + /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ + uPID->POn = POn; + uPID->POnE = (PIDPON_TypeDef)(POn == _PID_P_ON_E); + + uPID->DispKp = Kp; + uPID->DispKi = Ki; + uPID->DispKd = Kd; + + /* ~~~~~~~~~ Calculate time ~~~~~~~~ */ + SampleTimeInSec = ((float)uPID->SampleTime) / 1000; + + uPID->Kp = Kp; + uPID->Ki = Ki * SampleTimeInSec; + uPID->Kd = Kd / SampleTimeInSec; + + /* ~~~~~~~~ Check direction ~~~~~~~~ */ + if (uPID->ControllerDirection == _PID_CD_REVERSE) + { + + uPID->Kp = (0 - uPID->Kp); + uPID->Ki = (0 - uPID->Ki); + uPID->Kd = (0 - uPID->Kd); + + } + +} + +/* ~~~~~~~~~~~~~~~ PID Direction ~~~~~~~~~~~~~~~ */ +void PID_SetControllerDirection(PID_TypeDef *uPID, PIDCD_TypeDef Direction) +{ + /* ~~~~~~~~~~ Check parameters ~~~~~~~~~~ */ + if ((uPID->InAuto) && (Direction !=uPID->ControllerDirection)) + { + + uPID->Kp = (0 - uPID->Kp); + uPID->Ki = (0 - uPID->Ki); + uPID->Kd = (0 - uPID->Kd); + + } + + uPID->ControllerDirection = Direction; + +} +PIDCD_TypeDef PID_GetDirection(PID_TypeDef *uPID) +{ + return uPID->ControllerDirection; +} + +/* ~~~~~~~~~~~~~~~ PID Sampling ~~~~~~~~~~~~~~~~ */ +void PID_SetSampleTime(PID_TypeDef *uPID, int32_t NewSampleTime) +{ + + float ratio; + + /* ~~~~~~~~~~ Check value ~~~~~~~~~~ */ + if (NewSampleTime > 0) + { + + ratio = (float)NewSampleTime / (float)uPID->SampleTime; + + uPID->Ki *= ratio; + uPID->Kd /= ratio; + uPID->SampleTime = (uint32_t)NewSampleTime; + + } + +} + +/* ~~~~~~~~~~~~~ Get Tunings Param ~~~~~~~~~~~~~ */ +float PID_GetKp(PID_TypeDef *uPID) +{ + return uPID->DispKp; +} +float PID_GetKi(PID_TypeDef *uPID) +{ + return uPID->DispKi; +} +float PID_GetKd(PID_TypeDef *uPID) +{ + return uPID->DispKd; +} diff --git a/3rd-part/PID-Library/pid.h b/3rd-part/PID-Library/pid.h new file mode 100644 index 0000000..ebfddc6 --- /dev/null +++ b/3rd-part/PID-Library/pid.h @@ -0,0 +1,175 @@ + /* +------------------------------------------------------------------------------ +~ File : pid.h +~ Author : Majid Derhambakhsh +~ Version: V1.0.0 +~ Created: 02/11/2021 03:43:00 AM +~ Brief : +~ Support: + E-Mail : Majid.do16@gmail.com (subject : Embedded Library Support) + + Github : https://github.com/Majid-Derhambakhsh +------------------------------------------------------------------------------ +~ Description: + +~ Attention : + +~ Changes : +------------------------------------------------------------------------------ +*/ + +#ifndef __PID_H_ +#define __PID_H_ + +/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Include ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ +#include +#include + +// #include "zf_common_headfile.h" + +/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Defines ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ +/* ------------------------ Library ------------------------ */ +#define _PID_LIBRARY_VERSION 1.0.0 + +/* ------------------------ Public ------------------------- */ +#define _PID_8BIT_PWM_MAX UINT8_MAX +#define _PID_SAMPLE_TIME_MS_DEF 100 + +#ifndef _FALSE + + #define _FALSE 0 + +#endif + +#ifndef _TRUE + + #define _TRUE 1 + +#endif + +/* ---------------------- By compiler ---------------------- */ +#ifndef GetTime + + /* ---------------------- By compiler ---------------------- */ + + #ifdef __CODEVISIONAVR__ /* Check compiler */ + + #define GetTime() 0 + + /* ------------------------------------------------------------------ */ + + #elif defined(__GNUC__) /* Check compiler */ + + #define GetTime() 0 + + /* ------------------------------------------------------------------ */ + + + #else + #endif /* __CODEVISIONAVR__ */ + /* ------------------------------------------------------------------ */ + +#endif + +/* --------------------------------------------------------- */ + +/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Types ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ +/* PID Mode */ +typedef enum +{ + + _PID_MODE_MANUAL = 0, + _PID_MODE_AUTOMATIC = 1 + +}PIDMode_TypeDef; + +/* PID P On x */ +typedef enum +{ + + _PID_P_ON_M = 0, /* Proportional on Measurement */ + _PID_P_ON_E = 1 + +}PIDPON_TypeDef; + +/* PID Control direction */ +typedef enum +{ + + _PID_CD_DIRECT = 0, + _PID_CD_REVERSE = 1 + +}PIDCD_TypeDef; + +/* PID Structure */ +typedef struct +{ + + PIDPON_TypeDef POnE; + PIDMode_TypeDef InAuto; + + PIDPON_TypeDef POn; + PIDCD_TypeDef ControllerDirection; + + uint32_t LastTime; + uint32_t SampleTime; + + float DispKp; + float DispKi; + float DispKd; + + float Kp; + float Ki; + float Kd; + + float *MyInput; + float *MyOutput; + float *MySetpoint; + + float OutputSum; + float LastInput; + + float OutMin; + float OutMax; + +}PID_TypeDef; + +/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Variables ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ +/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Enum ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ +/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Struct ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ +/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Class ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ +/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 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); + +/* ::::::::::: Computing ::::::::::: */ +uint8_t PID_Compute(PID_TypeDef *uPID); + +/* ::::::::::: PID Mode :::::::::::: */ +void PID_SetMode(PID_TypeDef *uPID, PIDMode_TypeDef Mode); +PIDMode_TypeDef PID_GetMode(PID_TypeDef *uPID); + +/* :::::::::: PID Limits ::::::::::: */ +void PID_SetOutputLimits(PID_TypeDef *uPID, float Min, float Max); + +/* :::::::::: PID Tunings :::::::::: */ +void PID_SetTunings(PID_TypeDef *uPID, float Kp, float Ki, float Kd); +void PID_SetTunings2(PID_TypeDef *uPID, float Kp, float Ki, float Kd, PIDPON_TypeDef POn); + +/* ::::::::: PID Direction ::::::::: */ +void PID_SetControllerDirection(PID_TypeDef *uPID, PIDCD_TypeDef Direction); +PIDCD_TypeDef PID_GetDirection(PID_TypeDef *uPID); + +/* ::::::::: PID Sampling :::::::::: */ +void PID_SetSampleTime(PID_TypeDef *uPID, int32_t NewSampleTime); + +/* ::::::: Get Tunings Param ::::::: */ +float PID_GetKp(PID_TypeDef *uPID); +float PID_GetKi(PID_TypeDef *uPID); +float PID_GetKd(PID_TypeDef *uPID); + +/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ End of the program ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */ + +#endif /* __PID_H_ */ diff --git a/BC2D.ATWP b/BC2D.ATWP index 180d002..05cf2f3 100644 --- a/BC2D.ATWP +++ b/BC2D.ATWP @@ -31,20 +31,23 @@ - - - - - - - - + + + + + + + + + + + @@ -135,14 +138,12 @@ BC2D - C:/Users/ForgotDoge/Desktop + C:/Users/ForgotDoge/Desktop/BC2024/firmware MDK_V5 true 0x200 0x400 - true - true - C:/Users/ForgotDoge/Desktop/AT32_Work_Bench_V1.0.03/AT32F413_Firmware_Library_V2.1.3.zip - + false + diff --git a/BC2D.code-workspace b/BC2D.code-workspace index 8bb2331..55247a7 100644 --- a/BC2D.code-workspace +++ b/BC2D.code-workspace @@ -29,7 +29,8 @@ "activityBar.background": "#14117C", "titleBar.activeBackground": "#1D18AE", "titleBar.activeForeground": "#FAFAFE" - } + }, + "EIDE.OpenOCD.ExePath": "D:/Program Files (x86)/at32_OpenOCD_V2.0.2/bin/openocd.exe" }, "extensions": { "recommendations": [ diff --git a/app/by_motion.c b/app/by_motion.c new file mode 100644 index 0000000..52a5d34 --- /dev/null +++ b/app/by_motion.c @@ -0,0 +1,33 @@ +#include "by_motion.h" + +#include "dwt_delay.h" +#include "by_utils.h" + +#define DRV_ENABLE() gpio_bits_set(GPIOB, GPIO_PINS_15) +#define DRV_DISABLE() gpio_bits_reset(GPIOB, GPIO_PINS_15) + +void by_motion_pwm_m1(int32_t pwm_duty) +{ + pwm_duty = clip_s32(pwm_duty, -449, 449); // 不可以拉满哦 + pwm_duty += 499; + + // 互补 pwm 输出，499 为中值 + tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, pwm_duty); +} + +void by_motion_pwm_m2(int32_t pwm_duty) +{ + pwm_duty = clip_s32(pwm_duty, -449, 449); // 不可以拉满哦 + pwm_duty += 499; + + // 互补 pwm 输出，499 为中值 + tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, pwm_duty); +} + +void by_motion_init(void) +{ + DRV_ENABLE(); + by_motion_pwm_m1(125); + by_motion_pwm_m2(0); + +} diff --git a/app/by_motion.h b/app/by_motion.h new file mode 100644 index 0000000..076ca8f --- /dev/null +++ b/app/by_motion.h @@ -0,0 +1,8 @@ +#ifndef _BY_MOTION_H__ +#define _BY_MOTION_H__ + +#include "at32f413.h" + +void by_motion_init(void); + +#endif diff --git a/app/by_utils.c b/app/by_utils.c new file mode 100644 index 0000000..7e89e69 --- /dev/null +++ b/app/by_utils.c @@ -0,0 +1,7 @@ +#include "by_utils.h" + +inline int32_t clip_s32(int32_t x, int32_t low, int32_t up) +{ + return (x > up ? up : x < low ? low + : x); +} diff --git a/app/by_utils.h b/app/by_utils.h new file mode 100644 index 0000000..0ab18f0 --- /dev/null +++ b/app/by_utils.h @@ -0,0 +1,8 @@ +#ifndef _BY_UTILS_H__ +#define _BY_UTILS_H__ + +#include "at32f413.h" + +int32_t clip_s32(int32_t x, int32_t low, int32_t up); + +#endif \ No newline at end of file diff --git a/project/inc/at32f413_conf.h b/project/inc/at32f413_conf.h index 2199495..d88db5c 100644 --- a/project/inc/at32f413_conf.h +++ b/project/inc/at32f413_conf.h @@ -59,7 +59,7 @@ extern "C" { #define CRM_MODULE_ENABLED #define DEBUG_MODULE_ENABLED /*#define DMA_MODULE_ENABLED----------------------*/ -#define EXINT_MODULE_ENABLED +/*#define EXINT_MODULE_ENABLED--------------------*/ #define FLASH_MODULE_ENABLED #define GPIO_MODULE_ENABLED /*#define I2C_MODULE_ENABLED----------------------*/ diff --git a/project/inc/at32f413_wk_config.h b/project/inc/at32f413_wk_config.h index 0049160..4c017e2 100644 --- a/project/inc/at32f413_wk_config.h +++ b/project/inc/at32f413_wk_config.h @@ -74,9 +74,6 @@ extern "C" { /* init can1 function. */ void wk_can1_init(void); - /* init pwc function. */ - void wk_pwc_init(void); - /* init usart2 function. */ void wk_usart2_init(void); diff --git a/project/src/at32f413_wk_config.c b/project/src/at32f413_wk_config.c index 84d6b1b..96777ee 100644 --- a/project/src/at32f413_wk_config.c +++ b/project/src/at32f413_wk_config.c @@ -65,7 +65,7 @@ * @brief system clock config program * @note the system clock is configured as follow: * system clock (sclk) = hext / 2 * pll_mult - * system clock source = pll (hext) + * system clock source = HEXT_VALUE * - hext = HEXT_VALUE * - sclk = 200000000 * - ahbdiv = 1 @@ -182,9 +182,6 @@ void wk_periph_clock_config(void) /* enable can1 periph clock */ crm_periph_clock_enable(CRM_CAN1_PERIPH_CLOCK, TRUE); - - /* enable pwc periph clock */ - crm_periph_clock_enable(CRM_PWC_PERIPH_CLOCK, TRUE); } /** @@ -217,9 +214,17 @@ void wk_nvic_config(void) */ void wk_gpio_config(void) { + /* add user code begin gpio_config 0 */ + + /* add user code end gpio_config 0 */ + gpio_init_type gpio_init_struct; gpio_default_para_init(&gpio_init_struct); + /* add user code begin gpio_config 1 */ + + /* add user code end gpio_config 1 */ + /* gpio input config */ gpio_init_struct.gpio_mode = GPIO_MODE_INPUT; gpio_init_struct.gpio_pins = GPIO_PINS_3 | GPIO_PINS_4 | GPIO_PINS_5 | GPIO_PINS_6; @@ -235,46 +240,10 @@ void wk_gpio_config(void) gpio_init_struct.gpio_pins = GPIO_PINS_15; gpio_init_struct.gpio_pull = GPIO_PULL_NONE; gpio_init(GPIOB, &gpio_init_struct); -} -/** - * @brief init pwc function. - * @param none - * @retval none - */ -void wk_pwc_init(void) -{ - /* add user code begin pwc_init 0 */ + /* add user code begin gpio_config 2 */ - /* add user code end pwc_init 0 */ - - exint_init_type exint_init_struct; - - /* add user code begin pwc_init 1 */ - - /* add user code end pwc_init 1 */ - - /* set the threshold voltage*/ - pwc_pvm_level_select(PWC_PVM_VOLTAGE_2V8); - /* enable power voltage monitor */ - pwc_power_voltage_monitor_enable(TRUE); - - exint_default_para_init(&exint_init_struct); - exint_init_struct.line_enable = TRUE; - exint_init_struct.line_mode = EXINT_LINE_INTERRUPUT; - exint_init_struct.line_select = EXINT_LINE_16; - exint_init_struct.line_polarity = EXINT_TRIGGER_RISING_EDGE; - exint_init(&exint_init_struct); - - /** - * Users need to configure PWC interrupt functions according to the actual application. - * 1. Add the user's interrupt handler code into the below function in the at32f413_int.c file. - * --void PVM_IRQHandler (void) - */ - - /* add user code begin pwc_init 2 */ - - /* add user code end pwc_init 2 */ + /* add user code end gpio_config 2 */ } /** @@ -379,7 +348,7 @@ void wk_tmr1_init(void) gpio_init(GPIOA, &gpio_init_struct); /* configure counter settings */ - tmr_base_init(TMR1, 65535, 0); + tmr_base_init(TMR1, 999, 499); tmr_cnt_dir_set(TMR1, TMR_COUNT_UP); tmr_clock_source_div_set(TMR1, TMR_CLOCK_DIV1); tmr_repetition_counter_set(TMR1, 0); @@ -389,30 +358,37 @@ void wk_tmr1_init(void) tmr_sub_sync_mode_set(TMR1, FALSE); tmr_primary_mode_select(TMR1, TMR_PRIMARY_SEL_RESET); + /* configure overflow event */ + tmr_overflow_event_disable(TMR1, TRUE); + /* configure channel 1 output settings */ - tmr_output_struct.oc_mode = TMR_OUTPUT_CONTROL_OFF; + tmr_output_struct.oc_mode = TMR_OUTPUT_CONTROL_PWM_MODE_A; tmr_output_struct.oc_output_state = TRUE; tmr_output_struct.occ_output_state = TRUE; tmr_output_struct.oc_polarity = TMR_OUTPUT_ACTIVE_HIGH; tmr_output_struct.occ_polarity = TMR_OUTPUT_ACTIVE_HIGH; - tmr_output_struct.oc_idle_state = FALSE; + tmr_output_struct.oc_idle_state = TRUE; tmr_output_struct.occ_idle_state = FALSE; tmr_output_channel_config(TMR1, TMR_SELECT_CHANNEL_1, &tmr_output_struct); - tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, 0); + tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_1, 500); tmr_output_channel_buffer_enable(TMR1, TMR_SELECT_CHANNEL_1, FALSE); + tmr_output_channel_immediately_set(TMR1, TMR_SELECT_CHANNEL_1, FALSE); + /* configure channel 2 output settings */ - tmr_output_struct.oc_mode = TMR_OUTPUT_CONTROL_OFF; + tmr_output_struct.oc_mode = TMR_OUTPUT_CONTROL_PWM_MODE_A; tmr_output_struct.oc_output_state = TRUE; tmr_output_struct.occ_output_state = TRUE; tmr_output_struct.oc_polarity = TMR_OUTPUT_ACTIVE_HIGH; tmr_output_struct.occ_polarity = TMR_OUTPUT_ACTIVE_HIGH; - tmr_output_struct.oc_idle_state = FALSE; + tmr_output_struct.oc_idle_state = TRUE; tmr_output_struct.occ_idle_state = FALSE; tmr_output_channel_config(TMR1, TMR_SELECT_CHANNEL_2, &tmr_output_struct); - tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, 0); + tmr_channel_value_set(TMR1, TMR_SELECT_CHANNEL_2, 500); tmr_output_channel_buffer_enable(TMR1, TMR_SELECT_CHANNEL_2, FALSE); + tmr_output_channel_immediately_set(TMR1, TMR_SELECT_CHANNEL_2, FALSE); + /* configure break and dead-time settings */ tmr_brkdt_struct.brk_enable = FALSE; tmr_brkdt_struct.auto_output_enable = FALSE; diff --git a/project/src/main.c b/project/src/main.c index 2901280..8f2bb03 100644 --- a/project/src/main.c +++ b/project/src/main.c @@ -30,6 +30,8 @@ /* private includes ----------------------------------------------------------*/ /* add user code begin private includes */ #include "dwt_delay.h" + +#include "by_motion.h" /* add user code end private includes */ /* private typedef -----------------------------------------------------------*/ @@ -71,7 +73,7 @@ int main(void) { /* add user code begin 1 */ - /* add user code end 1 */ + /* add user code end 1 */ /* system clock config. */ wk_system_clock_config(); @@ -82,9 +84,6 @@ int main(void) /* init debug function. */ wk_debug_config(); - /* init pwc function. */ - wk_pwc_init(); - /* nvic config. */ wk_nvic_config(); @@ -110,16 +109,14 @@ int main(void) wk_gpio_config(); /* add user code begin 2 */ - DWT_Init(); - /* add user code end 2 */ + DWT_Init(); + + by_motion_init(); + /* add user code end 2 */ while(1) { /* add user code begin 3 */ - gpio_bits_reset(GPIOB, GPIO_PINS_15); - DWT_Delay(500000); - gpio_bits_set(GPIOB, GPIO_PINS_15); - DWT_Delay(500000); - /* add user code end 3 */ + /* add user code end 3 */ } }