代码工程
https://download.csdn.net/download/weixin_52849254/87879043?spm=1001.2014.3001.5501
PID算法实现
pid_algo.c
#include "pid_algo.h" #include "protocol.h" #include "stdio.h" //定义全局变量 t_pid t_pid_speed_left, t_pid_speed_right; t_pid t_pid_location_left, t_pid_location_right; /** * @brief PID参数初始化 * @note 无 * @retval 无 */ void PID_param_init() { /* 左电机位置相关初始化参数 */ t_pid_location_left.target_val=0.0; t_pid_location_left.actual_val=0.0; t_pid_location_left.err=0.0; t_pid_location_left.err_last=0.0; t_pid_location_left.integral=0.0; t_pid_location_left.Kp = 2.5; t_pid_location_left.Ki = 0.0; t_pid_location_left.Kd = 1.5; /* 左边电机速度初始化参数 */ t_pid_speed_left.target_val=0.0; t_pid_speed_left.actual_val=0.0; t_pid_speed_left.err=0.0; t_pid_speed_left.err_last=0.0; t_pid_speed_left.integral=0.0; t_pid_speed_left.Kp = 1.1; t_pid_speed_left.Ki = 0.68; t_pid_speed_left.Kd = 0.068; /* 右电机速度相关初始化参数 */ t_pid_speed_right.target_val=0.0; t_pid_speed_right.actual_val=0.0; t_pid_speed_right.err=0.0; t_pid_speed_right.err_last=0.0; t_pid_speed_right.integral=0.0; t_pid_speed_right.Kp = 0.965; t_pid_speed_right.Ki = 0.810; t_pid_speed_right.Kd = 0.667; /* 右电机位置相关初始化参数 */ t_pid_location_right.target_val=0.0; t_pid_location_right.actual_val=0.0; t_pid_location_right.err=0.0; t_pid_location_right.err_last=0.0; t_pid_location_right.integral=0.0; t_pid_location_right.Kp = 1.0; t_pid_location_right.Ki = 0.0; t_pid_location_right.Kd = 0.0; } /** * @brief 设置目标值 * @param val 目标值 * @note 无 * @retval 无 */ void set_pid_target(t_pid *pid,float temp_val) { pid->target_val = temp_val; // 设置当前的目标值 } /** * @brief 获取目标值 * @param 无 * @note 无 * @retval 目标值 */ float get_pid_target(t_pid *pid) { return pid->target_val; // 设置当前的目标值 } /** * @brief 设置比例、积分、微分系数 * @param p：比例系数 P * @param i：积分系数 i * @param d：微分系数 d * @note 无 * @retval 无 */ void set_p_i_d(t_pid *pid,float p, float i, float d) { pid->Kp = p; // 设置比例系数 P pid->Ki = i; // 设置积分系数 I pid->Kd = d; // 设置微分系数 D } /** * @brief 位置PID算法实现 * @param actual_val:实际值 * @note 无 * @retval 通过PID计算后的输出 */ float PID_location_realize(t_pid *pid, float actual_val) //位置环光个Kp好像也可以 { /*计算目标值与实际值的误差*/ pid->err=pid->target_val-actual_val; /* 设定闭环死区 */ //外环死区可以不要 if((pid->err >= -0.1) && (pid->err <= 0.1)) { pid->err = 0; pid->integral = 0; } pid->integral += pid->err; // 误差累积 /*PID算法实现*/ pid->actual_val = pid->Kp*pid->err +pid->Ki*pid->integral +pid->Kd*(pid->err-pid->err_last); /*误差传递*/ pid->err_last=pid->err; /*返回当前实际值*/ return pid->actual_val; } /** * @brief 速度PID算法实现 * @param actual_val:实际值 * @note 无 * @retval 通过PID计算后的输出 */ float PID_speed_realize(t_pid *pid,float actual_val) { /*计算目标值与实际值的误差*/ pid->err = pid->target_val - actual_val; if((pid->err<0.5f ) && (pid->err>-0.5f)) //假如以最大允许速度偏差运行1分钟，输出轴最大偏差为半圈 { pid->err = 0.0f; } pid->integral += pid->err;// 误差累积 /*积分限幅*/ if (pid->integral >= 1000) {pid->integral =1000;} else if (pid->integral < -1000) {pid->integral = -1000;} /*PID算法实现*/ pid->actual_val = pid->Kp * pid->err + pid->Ki * pid->integral + pid->Kd * (pid->err - pid->err_last); /*误差传递*/ pid->err_last = pid->err; /*返回当前实际值*/ return pid->actual_val; }
pid_algo.h
/* * pid_algo.h * * Created on: May 25, 2023 * Author: 榛� */ #ifndef PID_ALGO_H_ #define PID_ALGO_H_ #include "usart.h" #include <stdio.h> #include <stdlib.h> #include <math.h> #define PID_ASSISTANT_EN 1 typedef struct { float target_val; //目标值 float actual_val; //实际值 float err; //定义偏差值 float err_last; //定义上一个偏差值 float Kp,Ki,Kd; //定义比例、积分、微分系数 float integral; //定义积分值 }t_pid; extern t_pid t_pid_speed_left, t_pid_speed_right; extern t_pid t_pid_location_left,t_pid_location_right; void PID_param_init(void); void set_pid_target(t_pid *pid,float temp_val); float get_pid_target(t_pid *pid); void set_p_i_d(t_pid *pid, float p, float i, float d); float PID_location_realize(t_pid *pid, float actual_val); float PID_speed_realize(t_pid *pid,float actual_val); #endif /* PID_ALGO_H_ */
代码分析
这段代码是一个PID算法的实现，用于控制电机的位置和速度。代码中包含了以下主要内容：
定义了全局变量，包括四个PID结构体变量，分别用于左电机的位置控制、左电机的速度控制、右电机的速度控制和右电机的位置控制。
提供了PID参数初始化的函数
PID_param_init()，用于初始化各个PID结构体的参数。
提供了设置目标值的函数
set_pid_target()和获取目标值的函数get_pid_target()，用于设置和获取PID控制的目标值。
提供了设置比例、积分、微分系数的函数
set_p_i_d()，用于设置PID控制的比例、积分、微分系数。
提供了位置PID算法的实现函数
PID_location_realize()，接收实际值作为参数，根据PID算法计算输出值，并返回该输出值。
提供了速度PID算法的实现函数
PID_speed_realize()，接收实际值作为参数，根据PID算法计算输出值，并返回该输出值。
在代码中，位置PID算法根据目标值和实际值的误差，计算比例、积分和微分项，并将它们加权求和得到输出值。速度PID算法也是类似的，但还包括对积分项的限幅处理。
总体而言，这段代码实现了位置和速度的PID控制算法，并提供了相应的函数接口来设置参数和获取输出值。
电机控制
control.c
/* * control.c * * Created on: May 25, 2023 * Author: 榛� */ #include <string.h> #include "control.h" #include "usart.h" #include "tim.h" #include "pid_algo.h" #include "get_motor.h" #include "protocol.h" #include "control.h" #include "usart.h" #include "tim.h" #include "gpio.h" #include "stdio.h" E_CAR_STATE CAR_STATE; unsigned char location_control_count = 0; //执行频率不需要那么高的用这个事件计数 用在中断中 float Motor_left_PWM = 0.0, Motor_right_PWM = 0.0; static uint16_t duty_left_factor = 0; // 记录左电机占空比 static uint16_t duty_right_factor = 0; // 记录右电机占空比 uint8_t Spin_start_flag, Spin_succeed_flag, Stop_Flag; uint8_t Line_flag = 0, Turn_flag = 0; uint8_t stop_count, spin_count = 0; float speed_left_Outval, location_left_Outval; float speed_right_Outval, location_right_Outval; float Line_Outval; int Turn_val; /* 电机使能标志位 */ uint8_t motor_left_enable_flag = 0; uint8_t motor_right_enable_flag = 0; float g_fTargetJourney = 0; //存放小车左右轮所走路程和 ， 单位cm，需要在下一阶段任务中设置 float Motor_left_journey_cm, Motor_right_journey_cm; /*********************各PID**********************/ /****速度环位置环串级PID控制*****/ void Location_Speed_control(void) //原地转向可以直接 调用这个 { if (motor_left_enable_flag == 1 || motor_right_enable_flag == 1) // 电机在使能状态下才进行控制处理 { location_control_count++; if (location_control_count >= 2) { location_control_count = 0; location_left_Outval = location_left_pid_control(); location_right_Outval = location_right_pid_control(); //printf("location_left_Outval = %f ,location_right_Outval = %f",location_left_Outval,location_right_Outval); } //printf("speed_left_Outval = %f ,speed_right_Outval = %f",location_left_Outval,location_right_Outval); set_pid_target(&t_pid_speed_left, location_left_Outval); //每次都必须有位置环的值 //调试速度环PID时取消这两句 set_pid_target(&t_pid_speed_right, location_right_Outval); //每次都必须有位置环的值 speed_left_Outval = speed_left_pid_control(); //要是电机转向不符合预期，就在这两句里取反数值 // speed_right_Outval = speed_right_pid_control() + 1; speed_right_Outval = speed_right_pid_control(); } } float location_left_pid_control(void) { float cont_val = 0.0; int32_t actual_location; actual_location = g_lMotor_left_PulseSigma; //1圈 = 830个脉冲 = 计算（18.8*11*4） //这里位置用圈数代替。 // printf("g_lMotor_left_PulseSigma = %ld\r\n",g_lMotor_left_PulseSigma); cont_val = PID_location_realize(&t_pid_location_left, actual_location); //还没加串级pID之前，位置环的cont_val对应PWM。 改成串级PID后，位置换的cont_val对应目标速度 /* 目标速度上限处理 200*/ if (cont_val > TARGET_SPEED_MAX) { cont_val = TARGET_SPEED_MAX; } else if (cont_val < -TARGET_SPEED_MAX) { cont_val = -TARGET_SPEED_MAX; } //#if defined(PID_ASSISTANT_EN) // set_computer_value(SEND_FACT_CMD, CURVES_CH1, &actual_location, 1); // 给通道 1 发送实际值 //#endif return cont_val; } float location_right_pid_control(void) { float cont_val = 0.0; int32_t actual_location; actual_location = g_lMotor_right_PulseSigma; //1圈 = 830个脉冲 = 18.8*11*4 //这里位置用圈数代替。 // printf("g_lMotor_right_PulseSigma = %ld\r\n",g_lMotor_right_PulseSigma); cont_val = PID_location_realize(&t_pid_location_right, actual_location); // 改成串级PID后，位置换的cont_val对应目标速度 //目标速度限幅 /* 目标速度上限处理 */ if (cont_val > TARGET_SPEED_MAX) { cont_val = TARGET_SPEED_MAX; } else if (cont_val < -TARGET_SPEED_MAX) { cont_val = -TARGET_SPEED_MAX; } //#if defined(PID_ASSISTANT_EN) // set_computer_value(SEND_FACT_CMD, CURVES_CH1, &actual_location, 1); // 给通道 1 发送实际值 // //#endif return cont_val; } /* * 左边电机速度pid控制 */ float speed_left_pid_control(void) { float cont_val = 0.0; // 当前控制值 int32_t actual_speed; /* * 编码器物理分辨率 ENCODER_RESOLUTION 11 * 经过倍频之后的总分辨率 ENCODER_TOTAL_RESOLUTION (ENCODER_RESOLUTION * 4) * 减速电机减速比 REDUCTION_RATIO 18.8 * g_nMotor_left_Pulsed电机脉冲数 */ actual_speed = ((float) g_nMotor_left_Pulse * 1000.0 * 60.0) / (REDUCTION_ONE_CIRCLR * SPEED_PID_PERIOD); cont_val = PID_speed_realize(&t_pid_speed_left, actual_speed); // 进行 PID 计算 //#if defined(PID_ASSISTANT_EN) // set_computer_value(SEND_FACT_CMD, CURVES_CH1, &actual_speed, 1); // 给通道 1 发送实际值 //#endif return cont_val; } /* * 右边电机速度pid控制 */ float speed_right_pid_control(void) { float cont_val = 0.0; // 当前控制值 int32_t actual_speed; actual_speed = ((float) g_nMotor_right_Pulse * 1000.0 * 60.0) / (REDUCTION_ONE_CIRCLR * SPEED_PID_PERIOD); cont_val = PID_speed_realize(&t_pid_speed_right, actual_speed); // 进行 PID 计算 // //#if defined(PID_ASSISTANT_EN) // set_computer_value(SEND_FACT_CMD, CURVES_CH1, &actual_speed, 1); // 给通道 1 发送实际值 //#endif return cont_val; } /*****************电机的控制函数***************/ void MotorOutput(int nMotor_left_Pwm, int nMotor_right_Pwm) //设置电机电压和方向 { /* 设置电机的方向 * 前进 * 后退 */ if (nMotor_left_Pwm >= 0) // 判断电机方向 { move_left_forward(); //正方向要对应 } else { nMotor_left_Pwm = -nMotor_left_Pwm; move_left_backward(); //正方向要对应 } if (nMotor_right_Pwm >= 0) // 判断电机方向 { move_right_forward(); //正方向要对应 } else { nMotor_right_Pwm = -nMotor_right_Pwm; move_right_backward(); //正方向要对应 } nMotor_left_Pwm = (nMotor_left_Pwm > PWM_MAX_PERIOD_COUNT) ? PWM_MAX_PERIOD_COUNT : nMotor_left_Pwm; // 速度上限处理 nMotor_right_Pwm = (nMotor_right_Pwm > PWM_MAX_PERIOD_COUNT) ? PWM_MAX_PERIOD_COUNT : nMotor_right_Pwm; // 速度上限处理 speed_set_left_motor(nMotor_left_Pwm); // 设置 PWM 占空比 speed_set_right_motor(nMotor_right_Pwm); // 设置 PWM 占空比 } /************* 左边电机的控制 *******/ /** * @brief 设置电机速度 * @param v: 速度（占空比） * @retval 无 */ void speed_set_left_motor(uint16_t speed) { duty_left_factor = speed; SET_MOTOR_LEFT_COMPAER(duty_left_factor); } /** * @brief 设置电机方向 */ /* 前进，控制左边电机正转 */ void move_left_forward(void) { AIN1(1); AIN2(0); } /* 后退，控制左边电机反转 */ void move_left_backward(void) { AIN1(0); AIN2(1); } /** * @brief 使能左边电机 * @param 无 * @retval 无 */ void motor_left_enable(void) { motor_left_enable_flag = 1; MOTOR_LEFT_ENABLE(); } /** * @brief 禁用左边电机 * @param 无 * @retval 无 */ void motor_left_disable(void) { motor_left_enable_flag = 0; MOTOR_LEFT_DISABLE(); } /*****************右边电机的控制函数***************/ /** * @brief 设置右边电机速度 * @param speed: 速度（占空比） * @retval 无 */ /* 设置右边电机的速度 */ void speed_set_right_motor(uint16_t speed) { duty_right_factor = speed; SET_MOTOR_RIGHT_COMPAER(duty_right_factor); } /** * @brief 设置右边电机方向 */ /* 前进，控制左边电机正转 */ void move_right_forward(void) { BIN1(1); BIN2(0); } /* 后退，控制左边电机反转 */ void move_right_backward(void) { BIN1(0); BIN2(1); } /** * @brief 使能右边电机 * @param 无 * @retval 无 */ void motor_right_enable(void) { MOTOR_RIGHT_ENABLE(); motor_right_enable_flag = 1; } /** * @brief 禁用右边电机 * @param 无 * @retval 无 */ void motor_right_disable(void) { MOTOR_RIGHT_DISABLE(); motor_right_enable_flag = 0; } /** * @brief 设置电机方向 * @param 无 * @retval 无 */ /*这里加将位置环的输入直接编程距离（cm）的 函数*/ //所有普通直行都用这个 void Car_go(int32_t location_cm) //直走函数 //连续两次的直行好像会让巡线补偿失效 { float Car_location; Line_flag = 1; Stop_Flag = 0; Spin_start_flag = 0; Spin_succeed_flag = 0; Motor_left_journey_cm = 0; Motor_right_journey_cm = 0; how_way = 1; g_fTargetJourney = location_cm; //防止长时间PID控制用 //如果巡线效果不好就将3.142加大 Car_location = (location_cm / (2 * WheelR * 3.1416)) * (REDUCTION_ONE_CIRCLR); //Car_location 将 location_cm 转换为对应的脉冲数 g_fTargetJourney = 要转多少圈 * 1圈的脉冲数 //小车位置直接用一个电机的脉冲数累积就好，转向时不计数，开始一个位置前直接清零。 //打滑导致一个轮比另一个轮转位置不一样咋办,用“巡线环“弥补就好，转向就用“转向环” set_pid_target(&t_pid_location_left, Car_location); set_pid_target(&t_pid_location_right, Car_location); motor_left_enable(); //使能左边电机控制PWM输出 motor_right_enable(); //使能右边控制PWM输出 g_lMotor_left_PulseSigma = 0; //之前没清除所以用不了 g_lMotor_right_PulseSigma = 0; //move_stop(); } /*转角有左转90，右转90，和转180三种情况。*/ void spin_Turn(spin_dir_t zhuanxiang) //传入小车的轮距(mm) 186mm //其实转向环并不需要很精准，转弯后直接用直走时的巡线函数回正车身就好 { float spin90_val; float Car_Turn_val; Spin_start_flag = 1; Spin_succeed_flag = 0; how_way = 2; Line_flag = 0; //不进行巡线的补偿了 Stop_Flag = 0; //执行转弯时，将直走完成的标志位清零. 即如果上一次是直行，这次是转弯，则不用在业务代码里手动置位 Motor_left_journey_cm = 0; Motor_right_journey_cm = 0; spin90_val = 0.25 * 3.1416 * LUN_JU; /**************要是转弯后不能回到线上，可以改这里的转向系数**************/ if (zhuanxiang == left_90) //openmv识别到需要往左边病房走 { Car_Turn_val = (spin90_val / (2 * WheelR * 3.1416)) * (REDUCTION_ONE_CIRCLR); Car_Turn_val = 1.10 * Car_Turn_val; } else if (zhuanxiang == right_90){ //openmv识别到需要往右边病房走 Car_Turn_val = -(spin90_val / (2 * WheelR * 3.1416)) * (REDUCTION_ONE_CIRCLR); Car_Turn_val = 1.10 * Car_Turn_val; } else if (zhuanxiang == back_180) { Car_Turn_val = -(spin90_val / (2 * WheelR * 3.1416)) * (REDUCTION_ONE_CIRCLR); Car_Turn_val = 1.00 * Car_Turn_val; //同理 Car_Turn_val = 2.20 * Car_Turn_val; //0.96*180 = 175.5 } set_pid_target(&t_pid_location_left, -Car_Turn_val); set_pid_target(&t_pid_location_right, Car_Turn_val); g_lMotor_left_PulseSigma = 0; //之前没清除所以用不了，哈哈哈哈哈 g_lMotor_right_PulseSigma = 0; motor_left_enable(); //使能左边电机控制PWM输出 motor_right_enable(); //使能右边控制PWM输出 }
control.h
/* * control.h * * Created on: May 25, 2023 * Author: 榛� */ #ifndef CONTROL_H_ #define CONTROL_H_ #include "math.h" #include <stdio.h> #include <stdlib.h> #include "usart.h" #include "tim.h" #include "main.h" #include "stdint.h" extern uint8_t Line_flag , Turn_flag; /* 电机使能标志位 */ extern uint8_t Line_flag , Turn_flag ; extern uint8_t stop_count , spin_count; extern uint8_t Spin_start_flag , Spin_succeed_flag , Stop_Flag; extern uint8_t motor_left_enable_flag; extern uint8_t motor_right_enable_flag; extern float g_fTargetJourney; //存放小车左右轮所走路程和 ， 单位cm，需要在下一阶段任务中设置 extern float Line_Outval; extern int Turn_val; extern float Motor_left_journey_cm, Motor_right_journey_cm; extern unsigned char location_control_count; //执行频率不需要那么高的用这个事件计数 用在中断中 extern float Motor_left_PWM , Motor_right_PWM ; extern float speed_left_Outval, location_left_Outval; extern float speed_right_Outval, location_right_Outval; //pid调试 #define PID_ASSISTANT_EN 1 #define TUEN_RIGHT 2 #define TURN_LEFT 3 #define STOP 4 /* 小车状态枚举 */ typedef enum{ CAR_STOP = 0, CAR_FORWARD = 1, CAR_BACKWARD, CAR_TURN_LEFT, CAR_TURN_RIGHT, CAR_TURN_BACKWARD, }E_CAR_STATE; extern E_CAR_STATE CAR_STATE; /* 电机方向控制枚举 */ typedef enum { FORWARD = 0, BACKWARD, }MOTOR_DIRECTION; /* 累计 TIM_Period个后产生一个更新或者中断*/ /* 当定时器从0计数到PWM_PERIOD_COUNT，即为PWM_PERIOD_COUNT+1次，为一个定时周期 */ #define PWM_PERIOD_COUNT (1000) //可以尝试把这个变大，这样PID控制可以更顺滑 #define PWM2_PERIOD_COUNT (1000) /* 最大比较值 */ #define PWM_MAX_PERIOD_COUNT (PWM_PERIOD_COUNT - 30) //如果PWM弄成了满的，一些驱动板就会出现问题（硬件上的原因） #define PWM2_MAX_PERIOD_COUNT (PWM2_PERIOD_COUNT - 30) /* 编码器物理分辨率 */ #define ENCODER_RESOLUTION 11 /* 经过倍频之后的总分辨率 */ #define ENCODER_TOTAL_RESOLUTION (ENCODER_RESOLUTION * 4) /* 4倍频后的总分辨率 */ /* 减速电机减速比 */ #define REDUCTION_RATIO 18.8 /* * 一圈21.111502632123410562468963535638cm */ /* 电机一圈脉冲数为18.8*4*11 */ #define REDUCTION_ONE_CIRCLR (ENCODER_TOTAL_RESOLUTION * REDUCTION_RATIO) /*在sysTick里调用PID计算的周期，以毫秒为单位*/ #define SPEED_PID_PERIOD 20 //这个要看定时器7的中断周期 #define TARGET_SPEED_MAX 200 //60rpm可以3s走完60cm /*********************各PID**********************/ /****速度环位置环串级PID控制*****/ void Location_Speed_control(void); float location_left_pid_control(void); float speed_left_pid_control(void); float location_right_pid_control(void); float speed_right_pid_control(void); /*****************电机的控制函数***************/ void MotorOutput(int nMotor_left_Pwm, int nMotor_right_Pwm); /************* 左边电机的控制 *******/ /* 设置左边电机的速度 */ void speed_set_left_motor(uint16_t speed); /* 前进，控制左边电机正转 */ void move_left_forward(void); /* 后退，控制左边电机反转 */ void move_left_backward(void); /** * @brief 设置电机方向 * @param 无 * @retval 无 */ void set_left_motor_direction(MOTOR_DIRECTION dir); /** * @brief 使能左边电机 * @param 无 * @retval 无 */ void motor_left_enable(void); /** * @brief 禁用左边电机 * @param 无 * @retval 无 */ void motor_left_disable(void); /*****************右边电机的控制函数***************/ /** * @brief 设置右边电机速度 * @param speed: 速度（占空比） * @retval 无 */ /* 设置右边电机的速度 */ void speed_set_right_motor(uint16_t speed); /* 前进，控制左边电机正转 */ void move_right_forward(void); /* 后退，控制左边电机反转 */ void move_right_backward(void); /** * @brief 设置电机方向 * @param 无 * @retval 无 */ void set_right_motor_direction(MOTOR_DIRECTION dir); /** * @brief 使能右边电机 * @param 无 * @retval 无 */ void motor_right_enable(void); /** * @brief 禁用右边电机 * @param 无 * @retval 无 */ void motor_right_disable(void); #define LUN_JU 18.4 //单位c·m //轮子直径6.72cm #define WheelR 3.36 //单位cm #define HeadToWheelCM 12 #define WaitTime_ms 500 typedef enum { left_90, right_90, back_180 }spin_dir_t; void Car_go(int32_t location_cm); void spin_Turn(spin_dir_t zhuanxiang); #endif /* CONTROL_H_ */
代码分析
这段代码是一个控制小车运动的程序，主要包含了位置环和速度环的PID控制算法以及电机的控制函数。
代码中定义了一些全局变量和标志位，例如CAR_STATE表示小车的状态，location_control_count表示执行频率不需要那么高的事件计数，Motor_left_PWM和Motor_right_PWM表示左右电机的占空比，Spin_start_flag和Spin_succeed_flag表示转弯的开始和完成标志，Line_flag和Turn_flag表示巡线和转向标志等等。
函数Location_Speed_control是控制小车运动的主函数，它调用了location_left_pid_control和location_right_pid_control函数计算位置环的输出值，并将其作为目标速度传入speed_left_pid_control和speed_right_pid_control函数计算速度环的输出值。然后根据速度环的输出值设置左右电机的占空比，通过调用MotorOutput函数控制电机的转动。
函数location_left_pid_control和location_right_pid_control是位置环的PID控制函数，根据实际位置和目标位置计算控制量。函数speed_left_pid_control和speed_right_pid_control是速度环的PID控制函数，根据实际速度和目标速度计算控制量。
函数MotorOutput根据控制量设置电机的电压和方向，通过调用具体的电机控制函数来实现。
除了电机的控制函数外，还有一些辅助函数用于设置电机的速度、方向和使能状态。
整个代码实现了基于PID控制算法的小车运动控制，根据位置环和速度环的输出值控制电机的转动，从而实现小车的直行、转弯等动作。
电机编码获取
get_motor.c
/* * get_motor.c * * Created on: May 26, 2023 * Author: 黎 */ #include <stdio.h> #include "get_motor.h" #include "tim.h"//包含tim头文件 #include "gpio.h" long g_lMotor_left_PulseSigma =0;//电机25ms内累计脉冲总和 long g_lMotor_right_PulseSigma=0; short g_nMotor_left_Pulse=0,g_nMotor_right_Pulse=0;//全局变量， 保存电机脉冲数值 /*******************实际运行时读取编码器数值************************/ void GetMotorPulse(void)//读取电机脉冲 { g_nMotor_left_Pulse = (short)(__HAL_TIM_GET_COUNTER(&htim1));//获取计数器值 __HAL_TIM_SET_COUNTER(&htim1,0);//TIM1计数器清零 g_nMotor_right_Pulse = (short)(__HAL_TIM_GET_COUNTER(&htim8));//获取计数器值 g_nMotor_right_Pulse = -g_nMotor_right_Pulse; //测试下看看符不符合先 __HAL_TIM_SET_COUNTER(&htim8,0);//TIM8计数器清零 g_lMotor_left_PulseSigma += g_nMotor_left_Pulse;//位置外环使用的脉冲累积 //完成一次位置控制之后才清除。 g_lMotor_right_PulseSigma += g_nMotor_right_Pulse;//位置外环使用的脉冲累积 //记录了之后也要及时清零呀！！！ // printf("g_lMotor_left_PulseSigma = %ld\r\n",g_lMotor_left_PulseSigma); // printf("g_lMotor_right_PulseSigma = %ld\r\n",g_lMotor_right_PulseSigma); }
get_motor.h
/* * get_motor.h * * Created on: May 26, 2023 * Author: 黎 */ #ifndef GET_MOTOR_H_ #define GET_MOTOR_H_ extern long g_lMotor_left_PulseSigma;//电机25ms内累计脉冲总和 extern long g_lMotor_right_PulseSigma; extern short g_nMotor_left_Pulse,g_nMotor_right_Pulse;//全局变量 保存电机脉冲数值 void GetMotorPulse(void); #endif /* GET_MOTOR_H_ */
代码分析
这段代码是一个C语言文件，名为"get_motor.c"。它包含了一些函数用于读取电机的脉冲数值。下面是代码的解释：
g_lMotor_left_PulseSigma和
g_lMotor_right_PulseSigma是长整型变量，用于累计电机的脉冲总和。
g_nMotor_left_Pulse和
g_nMotor_right_Pulse是短整型变量，用于保存电机的脉冲数值。
GetMotorPulse函数用于读取电机的脉冲数值。具体流程如下：
首先，通过
__HAL_TIM_GET_COUNTER函数获取
htim1定时器的计数器值，并将其转换为短整型赋值给
g_nMotor_left_Pulse变量。
然后，通过
__HAL_TIM_SET_COUNTER函数将
htim1定时器的计数器清零。
接下来，通过
__HAL_TIM_GET_COUNTER函数获取
htim8定时器的计数器值，并将其转换为短整型赋值给
g_nMotor_right_Pulse变量。
再次，将
g_nMotor_right_Pulse变量取负值，赋值给自身。这一步可能是为了修正电机方向的问题。
最后，通过
__HAL_TIM_SET_COUNTER函数将
htim8定时器的计数器清零。
在函数的最后，将
g_nMotor_left_Pulse和
g_nMotor_right_Pulse分别累加到
g_lMotor_left_PulseSigma和
g_lMotor_right_PulseSigma中。
这段代码的作用是读取电机的脉冲数值，并进行一些累加和清零操作。
上位机通讯调试
protocol.c
/* * protocol.c * * Created on: May 25, 2023 * Author: 榛� */ #include "protocol.h" #include <string.h> #include "tim.h" #include "pid_algo.h" #include "protocol.h" #include <string.h> #include "control.h" #include "pid_algo.h" #include "tim.h" struct prot_frame_parser_t { uint8_t *recv_ptr; uint16_t r_oft; uint16_t w_oft; uint16_t frame_len; uint16_t found_frame_head; }; float set_point=0.0; float p_temp, i_temp, d_temp; static struct prot_frame_parser_t parser; static uint8_t recv_buf[PROT_FRAME_LEN_RECV]; /** * @brief 计算校验和 * @param ptr：需要计算的数据 * @param len：需要计算的长度 * @retval 校验和 */ uint8_t check_sum(uint8_t init, uint8_t *ptr, uint8_t len ) { uint8_t sum = init; while(len--) { sum += *ptr; ptr++; } return sum; } /** * @brief 得到帧类型（帧命令） * @param *frame: 数据帧 * @param head_oft: 帧头的偏移位置 * @return 帧长度. */ static uint8_t get_frame_type(uint8_t *frame, uint16_t head_oft) { return (frame[(head_oft + CMD_INDEX_VAL) % PROT_FRAME_LEN_RECV] & 0xFF); } /** * @brief 得到帧长度 * @param *buf: 数据缓冲区. * @param head_oft: 帧头的偏移位置 * @return 帧长度. */ static uint16_t get_frame_len(uint8_t *frame, uint16_t head_oft) { return ((frame[(head_oft + LEN_INDEX_VAL + 0) % PROT_FRAME_LEN_RECV] << 0) | (frame[(head_oft + LEN_INDEX_VAL + 1) % PROT_FRAME_LEN_RECV] << 8) | (frame[(head_oft + LEN_INDEX_VAL + 2) % PROT_FRAME_LEN_RECV] << 16) | (frame[(head_oft + LEN_INDEX_VAL + 3) % PROT_FRAME_LEN_RECV] << 24)); // 合成帧长度 } /** * @brief 获取 crc-16 校验值 * @param *frame: 数据缓冲区. * @param head_oft: 帧头的偏移位置 * @param head_oft: 帧长 * @return 帧长度. */ static uint8_t get_frame_checksum(uint8_t *frame, uint16_t head_oft, uint16_t frame_len) { return (frame[(head_oft + frame_len - 1) % PROT_FRAME_LEN_RECV]); } /** * @brief 查找帧头 * @param *buf: 数据缓冲区. * @param ring_buf_len: 缓冲区大小 * @param start: 起始位置 * @param len: 需要查找的长度 * @return -1：没有找到帧头，其他值：帧头的位置. */ static int32_t recvbuf_find_header(uint8_t *buf, uint16_t ring_buf_len, uint16_t start, uint16_t len) { uint16_t i = 0; for (i = 0; i < (len - 3); i++) { if (((buf[(start + i + 0) % ring_buf_len] << 0) | (buf[(start + i + 1) % ring_buf_len] << 8) | (buf[(start + i + 2) % ring_buf_len] << 16) | (buf[(start + i + 3) % ring_buf_len] << 24)) == FRAME_HEADER) { return ((start + i) % ring_buf_len); } } return -1; } /** * @brief 计算为解析的数据长度 * @param *buf: 数据缓冲区. * @param ring_buf_len: 缓冲区大小 * @param start: 起始位置 * @param end: 结束位置 * @return 为解析的数据长度 */ static int32_t recvbuf_get_len_to_parse(uint16_t frame_len, uint16_t ring_buf_len,uint16_t start, uint16_t end) { uint16_t unparsed_data_len = 0; if (start <= end) unparsed_data_len = end - start; else unparsed_data_len = ring_buf_len - start + end; if (frame_len > unparsed_data_len) return 0; else return unparsed_data_len; } /** * @brief 接收数据写入缓冲区 * @param *buf: 数据缓冲区. * @param ring_buf_len: 缓冲区大小 * @param w_oft: 写偏移 * @param *data: 需要写入的数据 * @param *data_len: 需要写入数据的长度 * @return void. */ static void recvbuf_put_data(uint8_t *buf, uint16_t ring_buf_len, uint16_t w_oft, uint8_t *data, uint16_t data_len) { if ((w_oft + data_len) > ring_buf_len) // 超过缓冲区尾 { uint16_t data_len_part = ring_buf_len - w_oft; // 缓冲区剩余长度 /* 数据分两段写入缓冲区*/ memcpy(buf + w_oft, data, data_len_part); // 写入缓冲区尾 memcpy(buf, data + data_len_part, data_len - data_len_part); // 写入缓冲区头 } else memcpy(buf + w_oft, data, data_len); // 数据写入缓冲区 } /** * @brief 查询帧类型（命令） * @param *data: 帧数据 * @param data_len: 帧数据的大小 * @return 帧类型（命令）. */ static uint8_t protocol_frame_parse(uint8_t *data, uint16_t *data_len) { uint8_t frame_type = CMD_NONE; uint16_t need_to_parse_len = 0; int16_t header_oft = -1; uint8_t checksum = 0; need_to_parse_len = recvbuf_get_len_to_parse(parser.frame_len, PROT_FRAME_LEN_RECV, parser.r_oft, parser.w_oft); // 得到为解析的数据长度 if (need_to_parse_len < 9) // 肯定还不能同时找到帧头和帧长度 return frame_type; /* 还未找到帧头，需要进行查找*/ if (0 == parser.found_frame_head) { /* 同步头为四字节，可能存在未解析的数据中最后一个字节刚好为同步头第一个字节的情况， 因此查找同步头时，最后一个字节将不解析，也不会被丢弃*/ header_oft = recvbuf_find_header(parser.recv_ptr, PROT_FRAME_LEN_RECV, parser.r_oft, need_to_parse_len); if (0 <= header_oft) { /* 已找到帧头*/ parser.found_frame_head = 1; parser.r_oft = header_oft; /* 确认是否可以计算帧长*/ if (recvbuf_get_len_to_parse(parser.frame_len, PROT_FRAME_LEN_RECV, parser.r_oft, parser.w_oft) < 9) return frame_type; } else { /* 未解析的数据中依然未找到帧头，丢掉此次解析过的所有数据*/ parser.r_oft = ((parser.r_oft + need_to_parse_len - 3) % PROT_FRAME_LEN_RECV); return frame_type; } } /* 计算帧长，并确定是否可以进行数据解析*/ if (0 == parser.frame_len) { parser.frame_len = get_frame_len(parser.recv_ptr, parser.r_oft); if(need_to_parse_len < parser.frame_len) return frame_type; } /* 帧头位置确认，且未解析的数据超过帧长，可以计算校验和*/ if ((parser.frame_len + parser.r_oft - PROT_FRAME_LEN_CHECKSUM) > PROT_FRAME_LEN_RECV) { /* 数据帧被分为两部分，一部分在缓冲区尾，一部分在缓冲区头 */ checksum = check_sum(checksum, parser.recv_ptr + parser.r_oft, PROT_FRAME_LEN_RECV - parser.r_oft); checksum = check_sum(checksum, parser.recv_ptr, parser.frame_len - PROT_FRAME_LEN_CHECKSUM + parser.r_oft - PROT_FRAME_LEN_RECV); } else { /* 数据帧可以一次性取完*/ checksum = check_sum(checksum, parser.recv_ptr + parser.r_oft, parser.frame_len - PROT_FRAME_LEN_CHECKSUM); } if (checksum == get_frame_checksum(parser.recv_ptr, parser.r_oft, parser.frame_len)) { /* 校验成功，拷贝整帧数据 */ if ((parser.r_oft + parser.frame_len) > PROT_FRAME_LEN_RECV) { /* 数据帧被分为两部分，一部分在缓冲区尾，一部分在缓冲区头*/ uint16_t data_len_part = PROT_FRAME_LEN_RECV - parser.r_oft; memcpy(data, parser.recv_ptr + parser.r_oft, data_len_part); memcpy(data + data_len_part, parser.recv_ptr, parser.frame_len - data_len_part); } else { /* 数据帧可以一次性取完*/ memcpy(data, parser.recv_ptr + parser.r_oft, parser.frame_len); } *data_len = parser.frame_len; frame_type = get_frame_type(parser.recv_ptr, parser.r_oft); /* 丢弃缓冲区中的命令帧*/ parser.r_oft = (parser.r_oft + parser.frame_len) % PROT_FRAME_LEN_RECV; } else { /* 校验错误，说明之前找到的帧头只是偶然出现的废数据*/ parser.r_oft = (parser.r_oft + 1) % PROT_FRAME_LEN_RECV; } parser.frame_len = 0; parser.found_frame_head = 0; return frame_type; } /** * @brief 接收数据处理 * @param *data: 要计算的数据的数组. * @param data_len: 数据的大小 * @return void. */ void protocol_data_recv(uint8_t *data, uint16_t data_len) { recvbuf_put_data(parser.recv_ptr, PROT_FRAME_LEN_RECV, parser.w_oft, data, data_len); // 接收数据 parser.w_oft = (parser.w_oft + data_len) % PROT_FRAME_LEN_RECV; // 计算写偏移 } /** * @brief 初始化接收协议 * @param void * @return 初始化结果. */ int32_t protocol_init(void) { memset(&parser, 0, sizeof(struct prot_frame_parser_t)); /* 初始化分配数据接收与解析缓冲区*/ parser.recv_ptr = recv_buf; return 0; } /** * @brief 接收的数据处理 * @param void * @return -1：没有找到一个正确的命令. */ int8_t receiving_process(void) { uint8_t frame_data[128]; // 要能放下最长的帧 uint16_t frame_len = 0; // 帧长度 uint8_t cmd_type = CMD_NONE; // 命令类型 while(1) { cmd_type = protocol_frame_parse(frame_data, &frame_len); switch (cmd_type) { case CMD_NONE: { return -1; } case SET_P_I_D_CMD: { uint32_t temp0 = COMPOUND_32BIT(&frame_data[13]); uint32_t temp1 = COMPOUND_32BIT(&frame_data[17]); uint32_t temp2 = COMPOUND_32BIT(&frame_data[21]); p_temp = *(float *)&temp0; i_temp = *(float *)&temp1; d_temp = *(float *)&temp2; // printf("p_temp = %f,i_temp = %f,d_temp = %f\r\n",p_temp,i_temp,d_temp); // set_p_i_d(&t_pid_location_left,p_temp, i_temp, d_temp); // 设置 P I D // set_p_i_d(&t_pid_location_right,p_temp, i_temp, d_temp); // 设置 P I D // set_p_i_d(&t_pid_speed_right,p_temp, i_temp, d_temp); // 设置 P I D // set_p_i_d(&t_pid_speed_left,p_temp, i_temp, d_temp); // 设置 P I D } break; case SET_TARGET_CMD: { int actual_temp = COMPOUND_32BIT(&frame_data[13]); // 得到数据 set_point = actual_temp; //printf("actual_temp = %d\r\n",actual_temp); set_pid_target(&t_pid_location_right,actual_temp); // 设置目标值 set_pid_target(&t_pid_location_left,actual_temp); // 设置目标值 // set_pid_target(&t_pid_speed_right,actual_temp); // 设置目标值 // set_pid_target(&t_pid_speed_left,actual_temp); // 设置目标值 } break; case START_CMD: { motor_right_enable(); motor_left_enable(); // 启动电机 } break; case STOP_CMD: { motor_right_disable(); motor_left_enable(); // 停止电机 } break; case RESET_CMD: { HAL_NVIC_SystemReset(); // 复位系统 } break; case SET_PERIOD_CMD: { // uint32_t temp = COMPOUND_32BIT(&frame_data[13]); // 周期数 // SET_BASIC_TIM_PERIOD(temp); // 设置定时器周期1~1000ms } break; default: return -1; } } } /** * @brief 设置上位机的值 * @param cmd：命令 * @param ch: 曲线通道 * @param data：参数指针 * @param num：参数个数 * @retval 无 */ void set_computer_value(uint8_t cmd, uint8_t ch, void *data, uint8_t num) { uint8_t sum = 0; // 校验和 num *= 4; // 一个参数 4 个字节 static packet_head_t set_packet; set_packet.head = FRAME_HEADER; // 包头 0x59485A53 set_packet.ch = ch; // 设置通道 set_packet.len = 0x0B + num; // 包长 set_packet.cmd = cmd; // 设置命令 sum = check_sum(0, (uint8_t *)&set_packet, sizeof(set_packet)); // 计算包头校验和 sum = check_sum(sum, (uint8_t *)data, num); // 计算参数校验和 HAL_UART_Transmit(&huart1, (uint8_t *)&set_packet, sizeof(set_packet), 0xFFFFF); // 发送数据头 HAL_UART_Transmit(&huart1, (uint8_t *)data, num, 0xFFFFF); // 发送参数 HAL_UART_Transmit(&huart1, (uint8_t *)&sum, sizeof(sum), 0xFFFFF); // 发送校验和 }
protocol.h
/* * protocol.h * * Created on: May 25, 2023 * Author: 榛� */ #ifndef PROTOCOL_H_ #define PROTOCOL_H_ #include "usart.h" #ifdef _cplusplus extern "C" { #endif #define PID_DEBUG_ON 1 #if PID_DEBUG_ON #define PID_DEBUG printf #else #definePID_DEBUG #endif /* 数据接收缓冲区大小 */ #define PROT_FRAME_LEN_RECV 128 /* 校验数据的长度 */ #define PROT_FRAME_LEN_CHECKSUM 1 extern float p_temp, i_temp, d_temp; /* 数据头结构体 *///__packed #pragma pack(1) typedef struct { uint32_t head; // 包头 uint8_t ch; // 通道 uint32_t len; // 包长度 uint8_t cmd; // 命令 // uint8_t sum; // 校验和 }packet_head_t; #pragma pack() #define FRAME_HEADER 0x59485A53 //0x59485A53 // 帧头 /* 通道宏定义 */ #define CURVES_CH1 0x01 #define CURVES_CH2 0x02 #define CURVES_CH3 0x03 #define CURVES_CH4 0x04 #define CURVES_CH5 0x05 /* 指令(下位机 -> 上位机) */ #define SEND_TARGET_CMD 0x01 // 发送上位机通道的目标值 #define SEND_FACT_CMD 0x02 // 发送通道实际值 #define SEND_P_I_D_CMD 0x03 // 发送 PID 值（同步上位机显示的值） #define SEND_START_CMD 0x04 // 发送启动指令（同步上位机按钮状态） #define SEND_STOP_CMD 0x05 // 发送停止指令（同步上位机按钮状态） #define SEND_PERIOD_CMD 0x06 // 发送周期（同步上位机显示的值） /* 指令(上位机 -> 下位机) */ #define SET_P_I_D_CMD 0x10 // 设置 PID 值 #define SET_TARGET_CMD 0x11 // 设置目标值 #define START_CMD 0x12 // 启动指令 #define STOP_CMD 0x13 // 停止指令 #define RESET_CMD 0x14 // 复位指令 #define SET_PERIOD_CMD 0x15 // 设置周期 /* 空指令 */ #define CMD_NONE 0xFF // 空指令 /* 索引值宏定义 */ #define HEAD_INDEX_VAL 0x3u // 包头索引值（4字节） #define CHX_INDEX_VAL 0x4u // 通道索引值（1字节） #define LEN_INDEX_VAL 0x5u // 包长索引值（4字节） #define CMD_INDEX_VAL 0x9u // 命令索引值（1字节） #define EXCHANGE_H_L_BIT(data) ((((data) << 24) & 0xFF000000) |\ (((data) << 8) & 0x00FF0000) |\ (((data) >> 8) & 0x0000FF00) |\ (((data) >> 24) & 0x000000FF)) // 交换高低字节 #define COMPOUND_32BIT(data) (((*(data-0) << 24) & 0xFF000000) |\ ((*(data-1) << 16) & 0x00FF0000) |\ ((*(data-2) << 8) & 0x0000FF00) |\ ((*(data-3) << 0) & 0x000000FF)) // 合成为一个字 /** * @brief 接收数据处理 * @param *data: 要计算的数据的数组. * @param data_len: 数据的大小 * @return void. */ void protocol_data_recv(uint8_t *data, uint16_t data_len); /** * @brief 初始化接收协议 * @param void * @return 初始化结果. */ int32_t protocol_init(void); /** * @brief 接收的数据处理 * @param void * @return -1：没有找到一个正确的命令. */ int8_t receiving_process(void); /** * @brief 设置上位机的值 * @param cmd：命令 * @param ch: 曲线通道 * @param data：参数指针 * @param num：参数个数 * @retval 无 */ void set_computer_value(uint8_t cmd, uint8_t ch, void *data, uint8_t num); #ifdef _cplusplus } #endif #endif /* PROTOCOL_H_ */
代码分析
上位机通信协议部分，用于解析接收到的数据帧并执行相应的操作。下面是对代码的逐行解释：
定义了一个用于帧解析的数据结构struct prot_frame_parser_t，包含了接收缓冲区的指针、读写偏移、帧长度等信息。
声明了一些全局变量和静态变量，包括一个接收缓冲区、一个设置点变量以及一些临时变量。
实现了一个计算校验和的函数check_sum，用于计算给定数据的校验和。
实现了一些辅助函数，用于从数据帧中获取帧类型、帧长度和校验和等信息。
实现了一个函数recvbuf_find_header，用于在接收缓冲区中查找帧头。
实现了一个函数recvbuf_get_len_to_parse，用于计算未解析的数据长度。
实现了一个函数recvbuf_put_data，用于将数据写入接收缓冲区。
实现了一个函数protocol_frame_parse，用于解析数据帧并返回帧类型。
实现了一个函数protocol_data_recv，用于将接收到的数据写入接收缓冲区。
实现了一个函数protocol_init，用于初始化协议解析相关的变量和缓冲区。
实现了一个函数receiving_process，用于处理接收到的数据帧。该函数通过调用
protocol_frame_parse函数解析数据帧，并根据帧类型执行相应的操作。
总体来说，这段代码是一个简单的嵌入式通信协议的实现，用于解析接收到的数据帧并执行相应的命令或操作。
定时器pid运算
float Motor_left_journey_cm, Motor_right_journey_cm; void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { /* Prevent unused argument(s) compilation warning */ UNUSED(htim); if (htim == &htim7) { GetMotorPulse(); Motor_left_journey_cm = (g_lMotor_left_PulseSigma / (REDUCTION_RATIO * ENCODER_TOTAL_RESOLUTION)) * (2 * WheelR * 3.1416); Motor_right_journey_cm = (g_lMotor_right_PulseSigma / (REDUCTION_RATIO * ENCODER_TOTAL_RESOLUTION)) * (2 * WheelR * 3.1416); Light_GoStraight_control(); //一直获取灰度传感器状态，但只有在执行car_go的时候，才进行补偿 /*****上位机pid调试速度话用,实际运行时注释掉*****/ /* * 左边电机调试，最终调试后 * P=0.7 * I=0.85 * D=0 */ // Motor_left_PWM = speed_left_pid_control(); // if (Motor_left_PWM >= 0) // 判断电机方向 // { // move_left_forward(); //正方向要对应 // }else{ // Motor_left_PWM = -Motor_left_PWM; // move_left_backward(); //正方向要对应 // } // speed_set_left_motor(Motor_left_PWM); /* * 右边电机调试，最终调试后 * P=0.967 * I=0.910 * D=0.567 */ // Motor_right_PWM = speed_right_pid_control(); // if (Motor_right_PWM >= 0) // 判断电机方向 // { // move_right_forward(); //正方向要对应 // }else{ // Motor_right_PWM = -Motor_right_PWM; // move_right_backward(); //正方向要对应 // } // speed_set_right_motor(Motor_right_PWM); /******/ /******上位机调试位置速度串级PID时用，实际使用时注释掉******/ // Location_Speed_control(); // //set_computer_value(SEND_FACT_CMD, CURVES_CH1, &g_lMotor_left_PulseSigma, 1); // //set_computer_value(SEND_FACT_CMD, CURVES_CH2, &g_lMotor_right_PulseSigma, 1); // Motor_left_PWM = speed_left_Outval; printf("Motor_left_PWM = %f\r\n",Motor_left_PWM); // Motor_right_PWM = speed_right_Outval; printf("Motor_right_PWM = %f\r\n",Motor_right_PWM); // MotorOutput(Motor_left_PWM,Motor_right_PWM); if (how_way == 1) { //来个判断，直行到达位置后停止PID控制，防止识别图片时或者等待装卸药时电机耗能，直走巡线结束的阶段才用这个， //其实这个要是用好一点的驱动模块的话，短时间内运行也不需要.....不用怕电机驱动过热...一直PID控制也没啥 // printf("Motor_left_journey_cm = %f Motor_right_journey_cm = %f\r\n",Motor_left_journey_cm,Motor_right_journey_cm); // printf("g_fTargetJourney = %f\r\n",g_fTargetJourney); if ((Motor_left_journey_cm <= (g_fTargetJourney + 24)) && (Motor_left_journey_cm >= (g_fTargetJourney - 24))) //这里偏差这么多真的好吗？并不是真的偏差 { stop_count++; //stop_count不能超过256 if (stop_count >= 100) //100 * 20 = 2s //可以时间判断放长点，以便刹车停稳 { Line_flag = 0; Stop_Flag = 1; //这个标志位可以用来判断是否执行下一阶段任务 stop_count = 0; motor_left_disable(); motor_right_disable(); } } else { Stop_Flag = 0; stop_count = 0; } if (motor_left_enable_flag == 1 && motor_right_enable_flag == 1) // 电机在使能状态下才进行控制处理 { Location_Speed_control(); //位置环速度环串级PID的输出是速度环输出的PWM值 if (Line_Num == 0) //每次回到线上需要补偿的时候，都将两个电机的累计脉冲数取平均值 //这个可能有问题 { long Pulse; Pulse = (g_lMotor_left_PulseSigma + g_lMotor_right_PulseSigma) / 2; g_lMotor_left_PulseSigma = Pulse; //可能有时候这里加上个补偿会更好 g_lMotor_right_PulseSigma = Pulse; } // //相当于只靠编码器去巡线，一直将两轮路程平均，比只回到线上才平均的效果更好！！！这个也会有帮助回到线上的效果 // long Pulse; // Pulse = (g_lMotorPulseSigma + g_lMotor2PulseSigma) / 2; // g_lMotorPulseSigma = Pulse; //可能有时候这里加上个补偿会更好 // g_lMotor2PulseSigma = Pulse; //这个是灰度传感器的巡线补偿 Line_Outval = Line_Num; //lineNum得在PWM的重装载值一半左右才会有明显的效果， 可以通过改变Line_Outval的值来控制补偿值的极性 Motor_left_PWM = speed_left_Outval + Line_Outval; Motor_right_PWM = speed_right_Outval - Line_Outval; MotorOutput(Motor_left_PWM, Motor_right_PWM); } } else if (how_way == 2) { Location_Speed_control(); //位置环速度环串级PID的输出是速度环输出的PWM值 Motor_left_PWM = speed_left_Outval; Motor_right_PWM = speed_right_Outval; spin_count++; if (spin_count >= 100) //20ms进入一次 100*20 = 2s，以能过够完成倒转时间为下限，应该已经倒转完毕了 { Spin_start_flag = 0; spin_count = 0; Spin_succeed_flag = 1; //Line_Num = 0.0; //转向时要避免记住上一次的补偿值吗？会影响到转弯后的巡线！！！ //在转向前有线的话，记住上一次的巡线补偿反而可以达到自回线的效果 //可以转向时转少一些！！至少让转向那一侧的灰度传感器打到线上，就可以达到自回线上的效果！！！ } MotorOutput(Motor_left_PWM, Motor_right_PWM); } } else if (htim == (&htim6)) { //1s进入一次TIM6的中断 } /* NOTE : This function should not be modified, when the callback is needed, the HAL_TIM_PeriodElapsedCallback could be implemented in the user file */ }
上面的代码是一个实现电机控制的函数，其中包含了PID控制、位置环和速度环的逻辑计算。
代码中的HAL_TIM_PeriodElapsedCallback函数是一个定时器中断回调函数，它在定时器溢出时被调用。在这个函数中，我们首先调用GetMotorPulse函数获取电机的脉冲数，并通过一系列计算将脉冲数转换为电机的行程距离（Motor_left_journey_cm和Motor_right_journey_cm）。
然后，根据how_way变量的值进行不同的控制逻辑。如果how_way为1，表示进行直线巡线控制。在这种情况下，我们检查电机行程距离是否接近目标位置，如果接近，则停止PID控制，并将Line_flag和Stop_Flag设置为相应的值。然后，我们根据灰度传感器的巡线补偿值（Line_Outval）和速度环的输出值（speed_left_Outval和speed_right_Outval）计算左右电机的PWM值，并调用MotorOutput函数输出控制信号。
如果how_way为2，表示进行转弯控制。在这种情况下，我们仍然进行位置环和速度环的控制，但没有巡线补偿。我们根据速度环的输出值计算左右电机的PWM值，并将其输出。
代码中还包含了一些注释掉的代码块，用于PID调试或其他目的。
综上所述，这段代码实现了电机控制的逻辑，包括直线巡线和转弯控制，并使用了PID控制、位置环和速度环来实现精确的控制。
如果文章对您有帮助，点赞支持👍，感谢🤝
