For basic configuration, see the previous serial port interrupt configuration.
-
Configure pwm
-
Configure encoder
-
Configure timer
-
Initialization configuration
-
Add pid file
-
Modify interrupt function
I want to control the position and speed of dual motors in cascade closed loop
| pwm | encoder | Enable pin | |
| Motor a | tim1 (PE9 PE12) | tim3 | PE10 |
| Motor b | tim8 (PC6 PC7) | tim4 |
PC8 |
first step
Wherever there are no screenshots, the default settings are
tim8 is the same as tim1
Step 2
The same goes for tim4 and tim3
third step
Configure the timer interrupt with the period you want. I made it 100hz, which means a timer interrupt every 10ms.
After the configuration is completed and the code is generated, if there is a problem compiling the new project, there is a solution in the previous article.
the fourth step
Add these before main loop
/*Turn on PWM output*/ HAL_TIM_PWM_Start( & amp;htim1,TIM_CHANNEL_1); HAL_TIM_PWM_Start( & amp;htim1,TIM_CHANNEL_2); HAL_TIM_PWM_Start( & amp;htim8,TIM_CHANNEL_1); HAL_TIM_PWM_Start( & amp;htim8,TIM_CHANNEL_2); /*Enable timer interrupt*/ HAL_TIM_Base_Start_IT( & amp;htim6); /*Open encoder capture*/ HAL_TIM_Encoder_Start( & amp;htim3,TIM_CHANNEL_ALL); HAL_TIM_Encoder_Start( & amp;htim4,TIM_CHANNEL_ALL); \t PID_param_init(); set_motor1_enable(); set_motor2_enable(); is_motor_en = 1; \t motor1_go(-30); motor2_go(30);
the fifth step
Add the c and h files of pid and the c and h files of control
pid c file
#include "bsp_pid.h"
//Define global variables
_pid pid_location1;
_pid pid_speed1;
_pid pid_location2;
_pid pid_speed2;
/**
* @brief PID parameter initialization
* @note None
* @retval None
*/
void PID_param_init(void)
{
/* 111 position related initialization parameters */
pid_location1.target_val=0;
pid_location1.actual_val=0.0;
pid_location1.err=0.0;
pid_location1.err_last=0.0;
pid_location1.integral=0.0;
pid_location1.Kp = 2;
pid_location1.Ki = 0.0;
pid_location1.Kd = 0.3;
/* 111 speed related initialization parameters */
pid_speed1.target_val=0.0;
pid_speed1.actual_val=0.0;
pid_speed1.err=0.0;
pid_speed1.err_last=0.0;
pid_speed1.integral=0.0;
pid_speed1.Kp = 3;
pid_speed1.Ki = 0.4;
pid_speed1.Kd = 0.01;
\t\t
/* 222 position related initialization parameters */
pid_location2.target_val=0;
pid_location2.actual_val=0.0;
pid_location2.err=0.0;
pid_location2.err_last=0.0;
pid_location2.integral=0.0;
pid_location2.Kp = 2;
pid_location2.Ki = 0.0;
pid_location2.Kd = 0.3;
/* 222 speed related initialization parameters */
pid_speed2.target_val=0.0;
pid_speed2.actual_val=0.0;
pid_speed2.err=0.0;
pid_speed2.err_last=0.0;
pid_speed2.integral=0.0;
pid_speed2.Kp = 3;
pid_speed2.Ki = 0.4;
pid_speed2.Kd = 0.01;
}
/**
* @brief Set target value
* @param val target value
* @note none
* @retval None
*/
void set_pid_target(_pid *pid, float temp_val)
{
pid->target_val = temp_val; //Set the current target value
}
/**
* @brief Get the target value
* @param None
* @note none
* @retval target value
*/
float get_pid_target(_pid *pid)
{
return pid->target_val; // current target value
}
/**
* @brief Set proportion, integral and differential coefficients
* @param p: proportional coefficient P
* @param i: integral coefficient i
* @param d: Differential coefficient d
* @note none
* @retval None
*/
void set_p_i_d(_pid *pid, float p, float i, float d)
{
pid->Kp = p; //Set the proportion coefficient P
pid->Ki = i; //Set the integral coefficient I
pid->Kd = d; //Set the differential coefficient D
}
/**
* @brief Position PID algorithm implementation
* @param actual_val: actual value
* @note none
* @retval Output calculated by PID
*/
float location_pid_realize_Motor1(_pid *pid, float actual_val)
{
/*Calculate the error between target value and actual value*/
pid->err=pid->target_val-actual_val;
/* Set closed-loop dead zone */
if((pid->err >= -20) & amp; & amp; (pid->err <= 20))
{
pid->err = 0;
pid->integral = 0;
}
pid->integral + = pid->err; // Error accumulation
/*PID algorithm implementation*/
pid->actual_val = pid->Kp*pid->err + pid->Ki*pid->integral + pid->Kd*(pid->err-pid->err_last);
/*Error propagation*/
pid->err_last=pid->err;
/*Return the current actual value*/
return pid->actual_val;
}
float location_pid_realize_Motor2(_pid *pid, float actual_val)
{
/*Calculate the error between target value and actual value*/
pid->err=pid->target_val-actual_val;
/* Set closed-loop dead zone */
if((pid->err >= -20) & amp; & amp; (pid->err <= 20))
{
pid->err = 0;
pid->integral = 0;
}
pid->integral + = pid->err; // Error accumulation
/*PID algorithm implementation*/
pid->actual_val = pid->Kp*pid->err + pid->Ki*pid->integral + pid->Kd*(pid->err-pid->err_last);
/*Error propagation*/
pid->err_last=pid->err;
/*Return the current actual value*/
return pid->actual_val;
}
/**
* @brief Speed PID algorithm implementation
* @param actual_val: actual value
* @note None
* @retval Output calculated by PID
*/
float speed_pid_realize(_pid *pid, float actual_val)
{
/*Calculate the error between target value and actual value*/
pid->err=pid->target_val-actual_val;
// if((pid->err<0.2f ) & amp; & amp; (pid->err>-0.2f))
// pid->err = 0.0f;
pid->integral + = pid->err; // Error accumulation
/*PID algorithm implementation*/
pid->actual_val = pid->Kp*pid->err + pid->Ki*pid->integral + pid->Kd*(pid->err-pid->err_last);
/*Error propagation*/
pid->err_last=pid->err;
/*Return the current actual value*/
return pid->actual_val;
}
/************************************ END OF FIER ********** *********************************************/
h file of pid
#ifndef __BSP_PID_H
#define __BSP_PID_H
#include "stm32f4xx.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#define PID_ASSISTANT_EN
typedef struct
{
float target_val; //Target value
float actual_val; //actual value
float err; //Define the deviation value
float err_last; //Define the previous deviation value
float Kp,Ki,Kd; //Define proportion, integral and differential coefficients
float integral; //define integral value
}_pid;
extern _pid pid_location1;
extern _pid pid_speed1;
extern _pid pid_location2;
extern _pid pid_speed2;
extern void PID_param_init(void);
extern void set_pid_target(_pid *pid, float temp_val);
extern float get_pid_target(_pid *pid);
extern void set_p_i_d(_pid *pid, float p, float i, float d);
extern float location_pid_realize_Motor1(_pid *pid, float actual_val);
extern float location_pid_realize_Motor2(_pid *pid, float actual_val);
extern float speed_pid_realize(_pid *pid, float actual_val);
#endif
contral c file
#include "tim.h"
#include "gpio.h"
#include "bsp_pid.h"
#include "contral.h"
#include <stdio.h>
#include <math.h>
#include "stdlib.h"
int mode1_flag=0,mode4_flag=1,mode4=0,num_jishu=1;
unsigned char location_control_count_Motor1 = 0; //If the execution frequency does not need to be so high, use this event count and use it in interrupts
unsigned char location_control_count_Motor2 = 0;
float Motor1PWM =0.0, Motor2PWM =0.0;
float speed_Outval_Motor1, speed_Outval_Motor2, location_Outval_Motor1, location_Outval_Motor2;
static motor_dir_t direction = MOTOR_FWD; // Record motor direction
static uint16_t dutyfactor = 0; // Record motor duty cycle
u8 is_motor_en = 0; // Motor enable
long g_lMotor1PulseSigma =0;
long g_lMotor2PulseSigma =0;
short g_nMotor1Pulse=0;//Global variable, saves the motor pulse value
short g_nMotor2Pulse=0;//Global variable, saves the motor pulse value
/************************Read the encoder value during actual operation************************ **/
void GetMotor1Pulse(void)//Read motor pulse
{
g_nMotor1Pulse = (short)(__HAL_TIM_GET_COUNTER( & amp;htim3));//Get the counter value
\t
__HAL_TIM_SET_COUNTER( & amp;htim3,0); //TIM3 counter clear
\t
g_lMotor1PulseSigma + = g_nMotor1Pulse;//Pulse accumulation used by the position outer loop
}
void GetMotor2Pulse(void)//Read motor pulse
{
g_nMotor2Pulse = (short)(__HAL_TIM_GET_COUNTER( & amp;htim4));//Get the counter value
\t
__HAL_TIM_SET_COUNTER( & amp;htim4,0);//TIM4 counter clear
\t
g_lMotor2PulseSigma + = g_nMotor2Pulse;//Pulse accumulation used by the position outer loop
}
/*Add here a function that directly programs the input of the position ring into distance (cm)*/ //This is used for all ordinary straight lines
void motor1_go(int32_t location_cm) //Straight travel function //Two consecutive straight travels seem to invalidate the line patrol compensation
{
float motor_location;
motor_location = ( location_cm / (6.5 * 3.14) ) * (45*4*13) ; //motor_location converts location_cm into the corresponding number of pulses g_fTargetJourney = how many turns * number of pulses in 1 turn
set_pid_target( & amp;pid_location1, motor_location);
set_motor1_enable(); //Enable motor control PWM output
// g_lMotorPulseSigma = 0; //It was not cleared before so it cannot be used.
}
void motor2_go(int32_t location_cm) //Straight travel function //Two consecutive straight travels seem to invalidate the line patrol compensation
{
float motor_location;
motor_location = ( location_cm / (6.5 * 3.14) ) * (45*4*13) ; //motor_location converts location_cm into the corresponding number of pulses g_fTargetJourney = how many turns * number of pulses in 1 turn
set_pid_target( & amp;pid_location2, motor_location);
set_motor2_enable(); //Enable motor control PWM output
// g_lMotorPulseSigma = 0; //It was not cleared before so it cannot be used.
}
/****Speed loop position loop cascade PID control*****/
void Location_Speed_control_Motor1(void)
{
if (is_motor_en == 1) // The motor is controlled only when it is enabled.
{
location_control_count_Motor1 + + ;
if(location_control_count_Motor1 >= 2)
{
location_control_count_Motor1 = 0;
location_Outval_Motor1 = location_pid_control_Motor1();
}
set_pid_target(&pid_speed1, location_Outval_Motor1); //There must be a position ring value every time
speed_Outval_Motor1 = speed_pid_control_Motor1(); //If the motor rotation is not as expected, take the inverse value in these two sentences
}
}
void Location_Speed_control_Motor2(void)
{
if (is_motor_en == 1) // The motor is controlled only when it is enabled.
{
location_control_count_Motor2 + + ;
if(location_control_count_Motor2 >= 2)
{
location_control_count_Motor2 = 0;
location_Outval_Motor2 = location_pid_control_Motor2();
}
set_pid_target( & amp;pid_speed2, location_Outval_Motor2); //There must be a position ring value every time
speed_Outval_Motor2 = speed_pid_control_Motor2(); //If the motor rotation is not as expected, take the inverse value in these two sentences
}
}
float location_pid_control_Motor1(void)
{
float cont_val = 0.0;
int32_t actual_location;
\t
actual_location = g_lMotor1PulseSigma; //1 circle = 2340 pulses = 45*13*4 //The position here is replaced by the number of circles.
cont_val = location_pid_realize_Motor1( & amp;pid_location1, actual_location);
/* Target speed upper limit processing */
if (cont_val > TARGET_SPEED_MAX)
{
cont_val = TARGET_SPEED_MAX;
}
else if (cont_val < -TARGET_SPEED_MAX)
{
cont_val = -TARGET_SPEED_MAX;
}
return cont_val;
}
float location_pid_control_Motor2(void)
{
float cont_val = 0.0;
int32_t actual_location;
\t
actual_location = g_lMotor2PulseSigma; //1 circle = 2340 pulses = 45*13*4 //The position here is replaced by the number of circles.
cont_val = location_pid_realize_Motor2( & amp;pid_location2, actual_location);
/* Target speed upper limit processing */
if (cont_val > TARGET_SPEED_MAX)
{
cont_val = TARGET_SPEED_MAX;
}
else if (cont_val < -TARGET_SPEED_MAX)
{
cont_val = -TARGET_SPEED_MAX;
}
return cont_val;
}
float speed_pid_control_Motor1(void)
{
float cont_val = 0.0; // current control value
int32_t actual_speed;
actual_speed = ((float)g_nMotor1Pulse*1000.0*60.0)/(4 * 13 * 45 * 20);
cont_val = speed_pid_realize( & amp;pid_speed1, actual_speed); // Perform PID calculation
\t
return cont_val;
}
float speed_pid_control_Motor2(void)
{
float cont_val = 0.0; // current control value
int32_t actual_speed;
actual_speed = ((float)g_nMotor2Pulse*1000.0*60.0)/(4 * 13 * 45 * 20);
cont_val = speed_pid_realize( & amp;pid_speed2, actual_speed); // Perform PID calculation
\t
return cont_val;
}
/************************Motor control function******************/
void Motor1Output(int nMotorPwm)//Set motor voltage and direction
{
if (nMotorPwm >= 0) // Determine the motor direction
{
set_motor1_direction(MOTOR_FWD); //The positive direction must correspond
}
else
{
nMotorPwm = -nMotorPwm;
set_motor1_direction(MOTOR_REV); //The positive direction must correspond
}
nMotorPwm = (nMotorPwm > PWM_MAX_PERIOD_COUNT) ? PWM_MAX_PERIOD_COUNT : nMotorPwm; // Speed upper limit processing
set_motor1_speed(nMotorPwm); //Set PWM duty cycle
}
void Motor2Output(int nMotorPwm)//Set motor voltage and direction
{
if (nMotorPwm >= 0) // Determine the motor direction
{
set_motor2_direction(MOTOR_FWD); //The positive direction must correspond
}
else
{
nMotorPwm = -nMotorPwm;
set_motor2_direction(MOTOR_REV); //The positive direction must correspond
}
nMotorPwm = (nMotorPwm > PWM_MAX_PERIOD_COUNT) ? PWM_MAX_PERIOD_COUNT : nMotorPwm; // Speed upper limit processing
set_motor2_speed(nMotorPwm); //Set PWM duty cycle
}
/**
* @brief Set motor speed
* @param v: speed (duty cycle)
* @retval None
*/
void set_motor1_speed(uint16_t v)
{
dutyfactor = v;
if (direction == MOTOR_FWD)
{
SET_FWD_COMPAER_Motor1(dutyfactor); // Set speed
SET_REV_COMPAER_Motor1(0);
}
else
{
SET_FWD_COMPAER_Motor1(0);
SET_REV_COMPAER_Motor1(dutyfactor); // Set speed
}
}
void set_motor2_speed(uint16_t v)
{
dutyfactor = v;
if (direction == MOTOR_FWD)
{
SET_FWD_COMPAER_Motor2(dutyfactor); // Set speed
SET_REV_COMPAER_Motor2(0);
}
else
{
SET_FWD_COMPAER_Motor2(0);
SET_REV_COMPAER_Motor2(dutyfactor); // Set speed
}
}
/**
* @brief Set motor direction
* @param None
* @retval None
*/
void set_motor1_direction(motor_dir_t dir)
{
direction = dir;
if (direction == MOTOR_FWD)
{
SET_FWD_COMPAER_Motor1(dutyfactor); // Set speed
SET_REV_COMPAER_Motor1(0); // Set speed
}
else
{
SET_FWD_COMPAER_Motor1(0); // Set speed
SET_REV_COMPAER_Motor1(dutyfactor); // Set speed
}
}
void set_motor2_direction(motor_dir_t dir)
{
direction = dir;
if (direction == MOTOR_FWD)
{
SET_FWD_COMPAER_Motor2(dutyfactor); // Set speed
SET_REV_COMPAER_Motor2(0); // Set speed
}
else
{
SET_FWD_COMPAER_Motor2(0); // Set speed
SET_REV_COMPAER_Motor2(dutyfactor); // Set speed
}
}
/**
* @brief enable motor
* @param None
* @retval None
*/
void set_motor1_enable(void) //Are these two enable and disable functions still valid for bipolar control?
{
is_motor_en = 1;
MOTOR1_ENABLE_SD();
MOTOR1_FWD_ENABLE();
MOTOR1_REV_ENABLE();
}
void set_motor2_enable(void) //Are these two enable and disable functions still valid for bipolar control?
{
is_motor_en = 1;
MOTOR2_ENABLE_SD();
MOTOR2_FWD_ENABLE();
MOTOR2_REV_ENABLE();
}
/**
* @brief disable motor
* @param None
* @retval None
*/
void set_motor1_disable(void)
{
is_motor_en = 0;
MOTOR1_DISABLE_SD();
MOTOR1_FWD_DISABLE();
MOTOR1_REV_DISABLE();
}
void set_motor2_disable(void)
{
is_motor_en = 0;
MOTOR2_DISABLE_SD();
MOTOR2_FWD_DISABLE();
MOTOR2_REV_DISABLE();
}
contral h file
#ifndef __CONTROL_H
#define __CONTROL_H
#include "main.h"
#include <stdio.h>
#include <stdlib.h>
#include "stm32f4xx.h"
#include "tim.h"
#include "bsp_pid.h"
extern _pid pid_location1, pid_location2;
extern float speed_Outval_Motor1, speed_Outval_Motor2, location_Outval_Motor1, location_Outval_Motor2;
extern u8 is_motor_en;
extern float Motor1PWM, Motor2PWM;
/* Motor SD or EN enable pin */
#define MOTOR1_ENABLE_SD() HAL_GPIO_WritePin(MOTOR1EN_GPIO_Port, MOTOR1EN_Pin, GPIO_PIN_SET) // High level on - high level enable
#define MOTOR1_DISABLE_SD() HAL_GPIO_WritePin(MOTOR1EN_GPIO_Port, MOTOR1EN_Pin, GPIO_PIN_RESET) // Low level shutdown - low level disable
#define MOTOR2_ENABLE_SD() HAL_GPIO_WritePin(MOTOR2EN_GPIO_Port, MOTOR2EN_Pin, GPIO_PIN_SET) // High level on - high level enable
#define MOTOR2_DISABLE_SD() HAL_GPIO_WritePin(MOTOR2EN_GPIO_Port, MOTOR2EN_Pin, GPIO_PIN_RESET) // Low level shutdown - low level disable
/* After accumulating TIM_Period times, an update or interrupt is generated*/
/* When the timer counts from 0 to PWM_PERIOD_COUNT, it is PWM_PERIOD_COUNT + 1 times, which is a timing period */
#define PWM_PERIOD_COUNT (4200) //You can try to make this larger so that PID control can be smoother
#define PWM2_PERIOD_COUNT (4200)
/* Maximum comparison value */
#define PWM_MAX_PERIOD_COUNT (PWM_PERIOD_COUNT - 100) //If the PWM becomes full, some driver boards will have problems (hardware reasons)
#define PWM2_MAX_PERIOD_COUNT (PWM2_PERIOD_COUNT - 100)
/****************Motor pin initialization******************/
/* Set speed (duty cycle) */
#define SET_FWD_COMPAER_Motor1(ChannelPulse) __HAL_TIM_SET_COMPARE( & amp;htim1,TIM_CHANNEL_1,ChannelPulse) // Set the value of the comparison register //AIN1
#define SET_REV_COMPAER_Motor1(ChannelPulse) __HAL_TIM_SET_COMPARE( & amp;htim1,TIM_CHANNEL_2,ChannelPulse) // Set the value of the comparison register //AIN2
#define SET_FWD_COMPAER_Motor2(ChannelPulse) __HAL_TIM_SET_COMPARE( & amp;htim8,TIM_CHANNEL_1,ChannelPulse) // Set the value of the comparison register //AIN1
#define SET_REV_COMPAER_Motor2(ChannelPulse) __HAL_TIM_SET_COMPARE( & amp;htim8,TIM_CHANNEL_2,ChannelPulse) // Set the value of the comparison register //AIN2
/* Enable output */
#define MOTOR1_FWD_ENABLE() HAL_TIM_PWM_Start( & amp;htim1,TIM_CHANNEL_1);
#define MOTOR1_REV_ENABLE() HAL_TIM_PWM_Start( & amp;htim1,TIM_CHANNEL_2);
#define MOTOR2_FWD_ENABLE() HAL_TIM_PWM_Start( & amp;htim8,TIM_CHANNEL_1);
#define MOTOR2_REV_ENABLE() HAL_TIM_PWM_Start( & amp;htim8,TIM_CHANNEL_2);
/* Disable output */
#define MOTOR1_FWD_DISABLE() HAL_TIM_PWM_Stop( & amp;htim1,TIM_CHANNEL_1);
#define MOTOR1_REV_DISABLE() HAL_TIM_PWM_Stop( & amp;htim1,TIM_CHANNEL_2);
#define MOTOR2_FWD_DISABLE() HAL_TIM_PWM_Stop( & amp;htim8,TIM_CHANNEL_1);
#define MOTOR2_REV_DISABLE() HAL_TIM_PWM_Stop( & amp;htim8,TIM_CHANNEL_2);
#define TARGET_SPEED_MAX 100 60rpm can cover 60cm in 3 seconds
/* Motor direction control enumeration */
typedef enum
{
MOTOR_FWD = 0,
MOTOR_REV,
}motor_dir_t;
void Location_Speed_control_Motor1(void);
void Location_Speed_control_Motor2(void);
float location_pid_control_Motor1(void);
float location_pid_control_Motor2(void);
float speed_pid_control_Motor1(void);
float speed_pid_control_Motor2(void);
void Motor1Output(int nMotorPwm);
void Motor2Output(int nMotorPwm);
void set_motor1_speed(uint16_t v);
void set_motor2_speed(uint16_t v);
void set_motor1_direction(motor_dir_t dir);
void set_motor2_direction(motor_dir_t dir);
void set_motor1_enable(void);
void set_motor1_disable(void);
void set_motor2_enable(void);
void set_motor2_disable(void);
void motor1_go(int32_t location_cm);
void motor2_go(int32_t location_cm);
void GetMotor1Pulse(void);
void GetMotor2Pulse(void);
#endif
I’m pretty rough with double, but in actual testing it works just fine.
Step 6
Modify the interrupt file as follows
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if(htim==( & amp;htim3))
{
}
else if(htim==( & amp;htim4))
{
}
else if(htim==( & amp;htim6))
{
GetMotor1Pulse();
GetMotor2Pulse();
if(is_motor_en == 1){
Location_Speed_control_Motor1();
Location_Speed_control_Motor2();
Motor1PWM = speed_Outval_Motor1;
Motor2PWM = speed_Outval_Motor2;
Motor1Output(Motor1PWM);
Motor2Output(Motor2PWM);
}
}
}
Then you can use this function to close the loop of the motor during initialization.
motor1_go(-30);
motor2_go(30);
There are a lot of notes in the project, and I have uploaded them all. If you need them, you can download them by yourself. You can find related services very cheaply by searching on a certain website.