Skip to content

Commit

Permalink
Modify most of the files in the algorithm folder to a format recogniz…
Browse files Browse the repository at this point in the history
…ed by Doxygen.
  • Loading branch information
ZhuYanzhen1 committed Oct 9, 2021
1 parent 872217c commit 1fdb661
Show file tree
Hide file tree
Showing 16 changed files with 188 additions and 148 deletions.
7 changes: 0 additions & 7 deletions program/algorithm/filter.c
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,6 @@ volatile Filter_Structure_t velocity_filter;
\param[in] cutoff_freq: cut off frequency in Hz
\param[in] sample_freq: sample frequency in Hz
\param[in] coefficient: input data weight, usually is 1
\param[out] none
\retval none
*/
void filter_coefficient_config(Filter_Structure_t *param, float cutoff_freq, float sample_freq, float coefficient) {
/* clear the value of the first order low pass filter parameter handler */
Expand All @@ -32,9 +30,6 @@ void filter_coefficient_config(Filter_Structure_t *param, float cutoff_freq, flo

/*!
\brief configure low-pass filter parameters
\param[in] none
\param[out] none
\retval none
*/
void filter_config(void) {
filter_coefficient_config((Filter_Structure_t *) &velocity_filter,
Expand All @@ -45,8 +40,6 @@ void filter_config(void) {
\brief update the output value of the first-order low-pass filter
\param[in] param: first order low pass filter parameter handler
\param[in] value: the sampled value to be filtered at this moment
\param[out] none
\retval none
*/
float filter_update_value(Filter_Structure_t *param, short value) {
/* judge whether the input parameters are abnormal */
Expand Down
22 changes: 15 additions & 7 deletions program/algorithm/filter.h
Original file line number Diff line number Diff line change
@@ -1,15 +1,23 @@
//
// Created by Lao·Zhu on 2021/9/5.
//
/**************************************************************************//**
\file filter.h
\brief this is the header file of filter.c, which defines the structure of filter algorithm.
\author Lao·Zhu
\version V1.0.1
\date 9. October 2021
******************************************************************************/

#ifndef MINIFOC_ALGORITHM_FILTER_H_
#define MINIFOC_ALGORITHM_FILTER_H_

/*!
\struct Filter_Structure_t
\brief structure of low-pass filter algorithm
*/
typedef struct {
float coefficient1;
float coefficient2;
float last_result;
float current_result;
float coefficient1; ///< low-pass filter coefficient 1
float coefficient2; ///< low-pass filter coefficient 2
float last_result; ///< previous calculation results of low-pass filter
float current_result; ///< current calculation results of low-pass filter
} Filter_Structure_t;

extern volatile Filter_Structure_t velocity_filter;
Expand Down
4 changes: 0 additions & 4 deletions program/algorithm/foc.c
Original file line number Diff line number Diff line change
Expand Up @@ -14,9 +14,6 @@ volatile unsigned char phase_sequence = 0;

/*!
\brief automatic phase sequence detection and correction
\param[in] none
\param[out] none
\retval none
*/
void foc_calibrate_phase(void) {
unsigned short last_angle = 0, positive_counter = 0;
Expand Down Expand Up @@ -80,7 +77,6 @@ void foc_calibrate_phase(void) {
\param[out] u: calculation results of U-phase duty cycle
\param[out] v: calculation results of V-phase duty cycle
\param[out] w: calculation results of W-phase duty cycle
\retval none
*/
void foc_calculate_dutycycle(float elect_angle, float d, float q, float *u, float *v, float *w) {
float alpha, beta;
Expand Down
24 changes: 18 additions & 6 deletions program/algorithm/foc.h
Original file line number Diff line number Diff line change
@@ -1,18 +1,30 @@
//
// Created by Lao·Zhu on 2021/8/30.
//
/**************************************************************************//**
\file foc.h
\brief this is the header file of foc.c, which defines the structure
of FOC algorithm and angle conversion factor.
\author Lao·Zhu
\version V1.0.1
\date 9. October 2021
******************************************************************************/

#ifndef MINIFOC_ALGORITHM_FOC_H_
#define MINIFOC_ALGORITHM_FOC_H_

/*!
\struct FOC_Structure_t
\brief structure of FOC algorithm
*/
typedef struct {
float mechanical_angle;
float rotate_speed;
float user_expect;
float mechanical_angle; ///< mechanical angle read form encoder
float rotate_speed; ///< motor rotate speed calculate from timer
float user_expect; ///< user expect value of miniFOC
} FOC_Structure_t;

/*! \brief mechanical angle conversion factor */
#define MECHANGLE_COEFFICIENT (6.2831854f / ENCODER_RESO)
/*! \brief electric angle conversion factor */
#define ELECANGLE_COEFFICIENT ((6.2831854f * POLAR_PAIRS) / ENCODER_RESO)
/*! \brief mechanical rotate speed conversion factor */
#define SPEED_COEFFICIENT ((6.2831852f * TIM13_FREQUENCY) / ENCODER_RESO)

extern volatile unsigned char phase_sequence;
Expand Down
31 changes: 22 additions & 9 deletions program/algorithm/pid.c
Original file line number Diff line number Diff line change
@@ -1,20 +1,34 @@
//
// Created by Lao·Zhu on 2021/9/11.
//
/**************************************************************************//**
\file pid.c
\brief PID algorithm implementation source code
\author Lao·Zhu
\version V1.0.1
\date 9. October 2021
******************************************************************************/

#include "pid.h"
#include "fast_math.h"
#include "system.h"

/*!
\brief flag variable for PID parameter availability
*/
unsigned char pid_parameter_available_flag = 0;
/*!
\brief flag variable of PID closed loop mode
*/
volatile unsigned char pid_control_mode_flag = 0;
volatile PID_Structure_t speed_pid_handler, angle_pid_handler;
/*!
\brief algorithm handler of PID speed loop
*/
volatile PID_Structure_t speed_pid_handler;
/*!
\brief algorithm handler of PID angle loop
*/
volatile PID_Structure_t angle_pid_handler;

/*!
\brief configure pid loop parameters
\param[in] none
\param[out] none
\retval none
\brief configure pid loop parameters
*/
void pid_config(unsigned char mode) {
/* clear the value of the PID handler */
Expand All @@ -34,7 +48,6 @@ void pid_config(unsigned char mode) {
\brief calculate result using sampling value
\param[in] pid_handler: PID data handler
\param[in] collect: sampled data
\param[out] none
\retval calculated output value of PID controller
*/
float pid_calculate_result(PID_Structure_t *pid_handler, float collect) {
Expand Down
39 changes: 26 additions & 13 deletions program/algorithm/pid.h
Original file line number Diff line number Diff line change
@@ -1,29 +1,42 @@
//
// Created by Lao·Zhu on 2021/9/11.
//
/**************************************************************************//**
\file pid.h
\brief this is the header file of pid.c, which defines the structure
of PID algorithm and closed-loop state macro.
\author Lao·Zhu
\version V1.0.1
\date 9. October 2021
******************************************************************************/

#ifndef MINIFOC_ALGORITHM_PID_H_
#define MINIFOC_ALGORITHM_PID_H_

/*! \brief torque loop control mode */
#define TORQUE_LOOP_CONTROL 1
/*! \brief speed loop control mode */
#define SPEED_LOOP_CONTROL 2
/*! \brief angle loop control mode */
#define ANGLE_LOOP_CONTROL 3

/*!
\struct PID_Structure_t
\brief structure of PID algorithm
*/
typedef struct {
float kp;
float ki;
float kd;
float summary;
float expect;
float maximum;
float minimum;
float sum_maximum;
float last_error;
float kp; ///< proportional term coefficient in PID
float ki; ///< integral term coefficient in PID
float kd; ///< differential term coefficient in PID
float summary; ///< value of integral term in PID
float expect; ///< user expectations in PID
float maximum; ///< maximum output in PID
float minimum; ///< minimum output in PID
float sum_maximum; ///< maximum of anti saturation integral in PID
float last_error; ///< error value of previous calculation in PID
} PID_Structure_t;

extern unsigned char pid_parameter_available_flag;
extern volatile unsigned char pid_control_mode_flag;
extern volatile PID_Structure_t speed_pid_handler, angle_pid_handler;
extern volatile PID_Structure_t speed_pid_handler;
extern volatile PID_Structure_t angle_pid_handler;
void pid_config(unsigned char mode);
float pid_calculate_result(PID_Structure_t *pid_handler, float collect);

Expand Down
4 changes: 2 additions & 2 deletions program/algorithm/report.c
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
#include "main.h"

/*!
\brief report PID parameters and current user settings
\brief report PID parameters and current user settings
*/
void report_local_variable(void) {
unsigned int upload_var[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
Expand Down Expand Up @@ -63,7 +63,7 @@ void report_local_variable(void) {
}

/*!
\brief report angle and speed using medium capacity transport protocol
\brief report angle and speed using medium capacity transport protocol
*/
void report_angle_speed(void) {
unsigned char buffer[8] = {0, 0, 0, 0, 0, 0, 0, 0};
Expand Down
10 changes: 7 additions & 3 deletions program/algorithm/report.h
Original file line number Diff line number Diff line change
@@ -1,6 +1,10 @@
//
// Created by Lao·Zhu on 2021/9/23.
//
/**************************************************************************//**
\file report.h
\brief report function header file
\author Lao·Zhu
\version V1.0.1
\date 9. October 2021
******************************************************************************/

#ifndef MINIFOC_ALGORITHM_REPORT_H_
#define MINIFOC_ALGORITHM_REPORT_H_
Expand Down
52 changes: 35 additions & 17 deletions program/config.h
Original file line number Diff line number Diff line change
@@ -1,26 +1,44 @@
//
// Created by Lao·Zhu on 2021/8/24.
//
/**************************************************************************//**
\file config.h
\brief used to place important parameter configurations for users
\author Lao·Zhu
\version V1.0.1
\date 9. October 2021
******************************************************************************/

#ifndef MINIFOC__CONFIG_H_
#define MINIFOC__CONFIG_H_

#define VBUS 8.4f // Bus voltage is 8.4V
#define POLAR_PAIRS 7 // BLDC motor has 7 polar pairs
#define ENCODER_RESO 4096 // Encoder resolution is 2^8=4096
#define CALI_TORQUE 0.3f // Calibrating torque is 0.3
/*! \brief bus voltage is 8.4V */
#define VBUS 8.4f
/*! \brief set BLDC polar pairs to 7 */
#define POLAR_PAIRS 7
/*! \brief SC60228 resolution is 2^8 = 4096 */
#define ENCODER_RESO 4096
/*! \brief set calibrate torque to 0.3 */
#define CALI_TORQUE 0.3f

#define UART_BAUDRATE 115200 // UART baudrate set to 115200
#define PWM_FREQUENCY 20 // PWM frequency set to 20kHz
#define TIM2_FREQUENCY 8 // timer2 interrupt frequency set to 8kHz
#define TIM13_FREQUENCY 100 // timer13 interrupt frequency set to 100Hz
#define SPEED_UP_FREQ 200 // speed update frequency set to 200Hz
#define SPI_PRESCALE SPI_PSC_16 // SPI frequency is 72 / 16 = 4.5MHz
/*! \brief set UART baud rate to 115200 */
#define UART_BAUDRATE 115200
/*! \brief set PWM frequency to 20kHz */
#define PWM_FREQUENCY 20
/*! \brief set FOC loop frequency to 8kHz */
#define TIM2_FREQUENCY 8
/*! \brief set speed/angle PID loop frequency to 100Hz */
#define TIM13_FREQUENCY 100
/*! \brief set speed update frequency to 200Hz */
#define SPEED_UP_FREQ 200
/*! \brief set SPI frequency to 72 / 16 = 4.5MHz */
#define SPI_PRESCALE SPI_PSC_16

#define UART_PRIORITY 1 // UART preemption priority set to 1
#define TIM2_PRIORITY 2 // TIMER2 preemption priority set to 3
#define TIM13_PRIORITY 3 // TIMER13 preemption priority set to 2
/*! \brief UART preemption priority set to 1 */
#define UART_PRIORITY 1
/*! \brief TIMER2 preemption priority set to 2 */
#define TIM2_PRIORITY 2
/*! \brief TIMER13 preemption priority set to 3 */
#define TIM13_PRIORITY 3

#define FMC_WRITE_START_ADDR 0x08007C00UL // User flash space start address, 1KB user flash
/*! \brief User flash space start address, 1KB user flash */
#define FMC_WRITE_START_ADDR 0x08007C00UL

#endif //MINIFOC__CONFIG_H_
10 changes: 7 additions & 3 deletions program/error.h
Original file line number Diff line number Diff line change
@@ -1,6 +1,10 @@
//
// Created by Lao·Zhu on 2021/9/18.
//
/**************************************************************************//**
\file error.h
\brief used to check whether the user configured parameters conflict
\author Lao·Zhu
\version V1.0.1
\date 9. October 2021
******************************************************************************/

#ifndef MINIFOC__ERROR_H_
#define MINIFOC__ERROR_H_
Expand Down
3 changes: 1 addition & 2 deletions program/main.c
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,6 @@ static volatile unsigned char minifoc_fsm_state = 0;
\brief user callback function for unpacking completion of medium capacity transport protocol
\param[in] pid: medium capacity transport protocol package id
\param[in] data: received data array of size 8 bytes
\retval none
*/
void mdtp_callback_handler(unsigned char pid, const unsigned char *data) {
/* pack0 is the control pack of miniFOC */
Expand Down Expand Up @@ -113,7 +112,7 @@ void mdtp_callback_handler(unsigned char pid, const unsigned char *data) {
}

/*!
\brief main function
\brief main function
*/
int main(void) {
/* 4 bits for preemption priority 0 bits for subpriority */
Expand Down
Loading

0 comments on commit 1fdb661

Please sign in to comment.