#define KEY1 HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_3)
#define KEY0 HAL_GPIO_ReadPin(GPIOE,GPIO_PIN_4)
#define PPR 13
#define ENCODER_MULTIPLY 4
#define GEAR_RATIO 20
// 计算每圈对应的计数
#define COUNTS_PER_REV (PPR * ENCODER_MULTIPLY * GEAR_RATIO)
#define PWM_MAX 1000 // PWM最大值
// 电机控制参数
float Kp1 = 3.8f, Ki1 = 1.1f, Kd1 = 0.015f; // PID参数
float Target_RPM1 = 0.0f; // 目标转速(RPM)
float Actual_RPM1 = 0.0f; // 实际转速(RPM)
float integral1 = 0.0f; // 积分项累积值
float Out1=0.0f;
// 电机控制参数
float Kp2 = 3.85f, Ki2 = 1.0f, Kd2 = 0.001f; // PID参数
float Target_RPM2 = 0.0f; // 目标转速(RPM)
float Actual_RPM2 = 0.0f; // 实际转速(RPM)
float integral2 = 0.0f; // 积分项累积值
float Out2=0.0f;
//float Target1=0, Actual1=0,Out1=0;
//float Kp1=1.5;
//float Ki1=0.02;
//float Kd1=0.1;
//float Error01=0,Error11=0,ErrorInt1=0,IntOut1=0;
//float Target2=0, Actual2=0,Out2=0;
//float Kp2=0.1;
//float Ki2=0.0;
//float Kd2=0.0;
//float Error02=0,Error12=0,ErrorInt2=0,IntOut2=0;
extern unsigned char Hzk[][32];
uint32_t ccr1_value=0;
uint32_t arr1_value;
uint32_t ccr2_value=0;
uint32_t arr2_value;
//unsigned int x=0;
//int motor_direction;
uint8_t num=0;
uint8_t rx_data;
//unsigned char* word1=(uint8_t *)"Speed1:";
//unsigned char* word2=(uint8_t *)"Target1:";
//unsigned char* word3=(uint8_t *)"Actual1:";
//unsigned char* word4=(uint8_t *)"Out1:";
//unsigned char* word5=(uint8_t *)"Speed2:";
//unsigned char* word6=(uint8_t *)"Target2:";
//unsigned char* word7=(uint8_t *)"Actual2:";
//unsigned char* word8=(uint8_t *)"Out2:";
uint16_t count1;
uint16_t count2;
float speed1=0.0;
float speed2=0.0;
//uint8_t str_buff[64];
//void Scan_Keys()
// {
// if(KEY0==GPIO_PIN_RESET)
// {
// HAL_Delay(100);
// if(KEY0==GPIO_PIN_RESET)
// {
// while(KEY0==GPIO_PIN_RESET);
// Target_RPM1+=130;
// Target_RPM2+=130;
// }
// }
// if(KEY1==GPIO_PIN_RESET)
// {
// HAL_Delay(100);
// if(KEY1==GPIO_PIN_RESET)
// {
// while(KEY1==GPIO_PIN_RESET);
// Target_RPM1-=100;
// Target_RPM2-=100;
// }
// }
// }
//void OLED_display_info()
//{
// OLED_Clear();
// OLED_ShowString(8,0,word1,sizeof(word1));
// OLED_ShowString(8,1,word2,sizeof(word2));
// OLED_ShowString(8,2,word3,sizeof(word3));
// OLED_ShowString(8,3,word4,sizeof(word4));
// OLED_ShowString(8,4,word5,sizeof(word5));
// OLED_ShowString(8,5,word6,sizeof(word6));
// OLED_ShowString(8,6,word7,sizeof(word7));
// OLED_ShowString(8,7,word8,sizeof(word8));
//// OLED_ShowString(8,4,word9,sizeof(word9));
//// OLED_ShowString(8,5,word10,sizeof(word10));
//// OLED_ShowString(8,6,word11,sizeof(word11));
//// OLED_ShowString(8,7,word12,sizeof(word12));
//// OLED_ShowCHinese(0,3,0);
//// OLED_ShowCHinese(18,3,1);
//// OLED_ShowCHinese(36,3,2);
//// OLED_ShowCHinese(54,3,3);
//// OLED_ShowCHinese(0,6,4);
//// OLED_ShowCHinese(18,6,5);
//// OLED_ShowCHinese(36,6,6);
//// OLED_ShowCHinese(54,6,7);
//}
//void OLED_display_dat()
//{
//// sprintf((char*)str_buff,"%5d",count);
//// OLED_ShowString(64,2,str_buff,sizeof(str_buff));
////
//// arr1_value = __HAL_TIM_GET_AUTORELOAD(&htim1);
//// float zkb = ((float)ccr1_value / arr1_value) * 100.0f;
////
//// sprintf((char*)str_buff,"%+.0f",zkb);
//// OLED_ShowString(64,3,str_buff,sizeof(str_buff));
// sprintf((char*)str_buff,"%+.0f",speed1);
// OLED_ShowString(64,0,str_buff,sizeof(str_buff));
//
// sprintf((char*)str_buff,"%+.0f",Target_RPM1);
// OLED_ShowString(64,1,str_buff,sizeof(str_buff));
//
// sprintf((char*)str_buff,"%+.0f",Actual_RPM1);
// OLED_ShowString(64,2,str_buff,sizeof(str_buff));
//
// sprintf((char*)str_buff,"%+.0f",Out1);
// OLED_ShowString(64,3,str_buff,sizeof(str_buff));
//
// sprintf((char*)str_buff,"%+.0f",speed2);
// OLED_ShowString(64,4,str_buff,sizeof(str_buff));
//
// sprintf((char*)str_buff,"%+.0f",Target_RPM2);
// OLED_ShowString(64,5,str_buff,sizeof(str_buff));
//
// sprintf((char*)str_buff,"%+.0f",Actual_RPM2);
// OLED_ShowString(64,6,str_buff,sizeof(str_buff));
//
// sprintf((char*)str_buff,"%+.0f",Out2);
// OLED_ShowString(64,7,str_buff,sizeof(str_buff));
//}
//uint16_t last_count1 = 0;
//uint16_t last_time_ms1 = 0;
//uint16_t last_count2 = 0;
//uint16_t last_time_ms2 = 0;
//void calculate_speed_1()
//{
// count1 = __HAL_TIM_GET_COUNTER(&htim3);
// uint16_t current_time_ms1 = HAL_GetTick();
// int16_t diff_count1 = count1 - last_count1;
// float delta_time_sec1 = (current_time_ms1 - last_time_ms1) / 1000.0f;
//
// if (delta_time_sec1 > 0)
// {
// float rpm1 = ((float)diff_count1 / (float)COUNTS_PER_REV) * 60.0f / delta_time_sec1;
//
// speed1 = rpm1;
//
// Actual1=rpm1/2530.0f*100;
//
// Error11 = Error01;
// Error01 = Target1 - Actual1;
//
//// ErrorInt += Error0;
// IntOut1 += Ki1 * Error01;
//
// if(IntOut1>800)IntOut1=800;
// if(IntOut1<0)IntOut1=0;
//
// Out1 = Kp1 * Error01 + IntOut1 + Kd1 * (Error01 - Error11);
//
// float OUT1 = Out1 * 8.0;
//
// if(OUT1 > 800)OUT1 = 800;
// if(OUT1 < 0)OUT1 = 0;
//
//// __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,OUT1);
// }
//
// last_count1 = count1;
// last_time_ms1 = current_time_ms1;
//}
//void calculate_speed_2()
//{
// count2 = __HAL_TIM_GET_COUNTER(&htim4);
// uint16_t current_time_ms2 = HAL_GetTick();
// int16_t diff_count2 = count2 - last_count2;
// float delta_time_sec2 = (current_time_ms2 - last_time_ms2) / 1000.0f;
//
// if (delta_time_sec2 > 0)
// {
// float rpm2 = ((float)diff_count2 / (float)COUNTS_PER_REV) * 60.0f / delta_time_sec2;
//
// speed2 = rpm2;
//
// Actual2=rpm2/2530.0f*100;
//
// Error12 = Error02;
// Error02 = Target2 - Actual2;
//// ErrorInt += Error0;
// IntOut2 += Ki2 * Error02;
//
// if(IntOut2>800)IntOut2=800;
// if(IntOut2<0)IntOut2=0;
//
// Out2 = Kp2 * Error02 + IntOut2 + Kd2 * (Error02 - Error12);
//
// float OUT2 = Out2 * 8.0;
//
// if(OUT2 > 800)OUT2 = 800;
// if(OUT2 < 0)OUT2 = 0;
//
//// __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1,OUT2);
// }
// last_count2 = count2;
// last_time_ms2 = current_time_ms2;
//}
void calculate_speed_1()
{
static uint32_t last_count1 = 0;
static uint32_t last_time_ms1 = 0;
static float prev_error1 = 0.0f;
uint32_t current_count1 = __HAL_TIM_GET_COUNTER(&htim3); // 编码器计数器
uint32_t current_time_ms1 = HAL_GetTick(); // 当前时间戳
// 计算时间差(秒)
float delta_time_sec1 = (current_time_ms1 - last_time_ms1) / 1000.0f;
if (delta_time_sec1 > 0.001f)
{
// 计算实际RPM值
int32_t diff_count1 = (int32_t)(current_count1 - last_count1);
Actual_RPM1 = (diff_count1 / (float)COUNTS_PER_REV) * 60.0f / delta_time_sec1;
// PID计算
float error1 = Target_RPM1 - fabs(Actual_RPM1);
// 比例项
float P1 = Kp1 * error1;
// 积分项(带抗饱和)
integral1 += Ki1 * error1 * delta_time_sec1;
if(integral1 > PWM_MAX) integral1 = PWM_MAX;
else if(integral1 < 0) integral1 = 0;
// 微分项
float D1 = Kd1 * (error1 - prev_error1) / delta_time_sec1;
prev_error1 = error1;
// PID输出
Out1 = P1 + integral1 + D1;
// PWM限幅
if(Out1 > 1000)Out1 = 1000;
if(Out1 < 0)Out1 = 0;
__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, (uint32_t)Out1);
}
last_count1 = current_count1;
last_time_ms1 = current_time_ms1;
}
void calculate_speed_2()
{
static uint32_t last_count2 = 0;
static uint32_t last_time_ms2 = 0;
static float prev_error2 = 0.0f;
uint32_t current_count2 = __HAL_TIM_GET_COUNTER(&htim4); // 编码器计数器
uint32_t current_time_ms2 = HAL_GetTick(); // 当前时间戳
// 计算时间差(秒)
float delta_time_sec2 = (current_time_ms2 - last_time_ms2) / 1000.0f;
if (delta_time_sec2 > 0.001f)
{
// 计算实际RPM值
int32_t diff_count2 = (int32_t)(current_count2 - last_count2);
Actual_RPM2 = (diff_count2 / (float)COUNTS_PER_REV) * 60.0f / delta_time_sec2;
// PID计算
float error2 = Target_RPM2 - fabs(Actual_RPM2);
// 比例项
float P2 = Kp2 * error2;
// 积分项(带抗饱和)
integral2 += Ki2 * error2 * delta_time_sec2;
if(integral2 > PWM_MAX) integral2 = PWM_MAX;
else if(integral2 < 0) integral2 = 0;
// 微分项
float D2 = Kd2 * (error2 - prev_error2) / delta_time_sec2;
prev_error2 = error2;
// PID输出
Out2 = P2 + integral2 + D2;
// PWM限幅
if(Out2 > 1000)Out2 = 1000;
if(Out2 < 0)Out2 = 0;
__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, (uint32_t)Out2);
}
last_count2 = current_count2;
last_time_ms2 = current_time_ms2;
}
//float Kc = 0.6f; // 同步补偿增益(建议0.3~0.8倍Kp)
//void calculate_speed_1()
//{
// static uint32_t last_count1 = 0;
// static uint32_t last_time_ms1 = 0;
// static float prev_error1 = 0.0f;
// static float integral1 = 0.0f;
//
// uint32_t current_count1 = __HAL_TIM_GET_COUNTER(&htim3);
// uint32_t current_time_ms1 = HAL_GetTick();
//
// float delta_time_sec1 = (current_time_ms1 - last_time_ms1) / 1000.0f;
//
// if (delta_time_sec1 > 0.001f)
// {
// // 计算实际RPM(带方向检测)
// int32_t diff_count1 = (int32_t)(current_count1 - last_count1);
// Actual_RPM1 = (diff_count1 / (float)COUNTS_PER_REV) * 60.0f / delta_time_sec1;
//
// // PID计算
// float error1 = Target_RPM1 - Actual_RPM1;
//
// // 比例项
// float P1 = Kp1 * error1;
//
// // 积分项(带抗饱和)
// integral1 += Ki1 * error1 * delta_time_sec1;
// if(integral1 > PWM_MAX) integral1 = PWM_MAX;
// else if(integral1 < -PWM_MAX) integral1 = -PWM_MAX;
//
// // 微分项
// float D1 = Kd1 * (error1 - prev_error1) / delta_time_sec1;
// prev_error1 = error1;
//
// // 基础PID输出
// float base_out1 = P1 + integral1 + D1;
//
// // 同步补偿项(关键改进)
// float sync_term = Kc * (Actual_RPM2 - Actual_RPM1);
//
// // 总输出 = 基础PID + 同步补偿
// Out1 = base_out1 + sync_term;
//
// // PWM限幅
// if(Out1 > PWM_MAX) Out1 = PWM_MAX;
// else if(Out1 < 0) Out1 = 0;
//
// __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, (uint32_t)Out1);
// }
//
// last_count1 = current_count1;
// last_time_ms1 = current_time_ms1;
//}
//void calculate_speed_2()
//{
// static uint32_t last_count2 = 0;
// static uint32_t last_time_ms2 = 0;
// static float prev_error2 = 0.0f;
// static float integral2 = 0.0f;
//
// uint32_t current_count2 = __HAL_TIM_GET_COUNTER(&htim4);
// uint32_t current_time_ms2 = HAL_GetTick();
//
// float delta_time_sec2 = (current_time_ms2 - last_time_ms2) / 1000.0f;
//
// if (delta_time_sec2 > 0.001f)
// {
// // 计算实际RPM(带方向检测)
// int32_t diff_count2 = (int32_t)(current_count2 - last_count2);
// Actual_RPM2 = (diff_count2 / (float)COUNTS_PER_REV) * 60.0f / delta_time_sec2;
//
// // PID计算
// float error2 = Target_RPM2 - Actual_RPM2;
//
// // 比例项
// float P2 = Kp2 * error2;
//
// // 积分项(带抗饱和)
// integral2 += Ki2 * error2 * delta_time_sec2;
// if(integral2 > PWM_MAX) integral2 = PWM_MAX;
// else if(integral2 < -PWM_MAX) integral2 = -PWM_MAX;
//
// // 微分项
// float D2 = Kd2 * (error2 - prev_error2) / delta_time_sec2;
// prev_error2 = error2;
//
// // 基础PID输出
// float base_out2 = P2 + integral2 + D2;
//
// // 同步补偿项(关键改进)
// float sync_term = Kc * (Actual_RPM1 - Actual_RPM2);
//
// // 总输出 = 基础PID + 同步补偿
// Out2 = base_out2 + sync_term;
//
// // PWM限幅
// if(Out2 > PWM_MAX) Out2 = PWM_MAX;
// else if(Out2 < 0) Out2 = 0;
//
// __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, (uint32_t)Out2);
// }
//
// last_count2 = current_count2;
// last_time_ms2 = current_time_ms2;
//}
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_DMA_Init();
// MX_I2C1_Init();
MX_TIM2_Init();
MX_TIM3_Init();
MX_TIM4_Init();
// MX_TIM5_Init();
// MX_USART1_UART_Init();
MX_ADC1_Init();
/* USER CODE BEGIN 2 */
// OLED_Init();
// OLED_display_info();
// 启动编码器
HAL_TIM_Encoder_Start(&htim3,TIM_CHANNEL_ALL);
HAL_TIM_Encoder_Start(&htim4,TIM_CHANNEL_ALL);
// // 启动串口接收中断
// HAL_UART_Receive_IT(&huart1, &rx_data, 1);
// // 启动定时器并启用中断
// HAL_TIM_Base_Start_IT(&htim5);
HAL_GPIO_WritePin(GPIOE,GPIO_PIN_14, GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOE,GPIO_PIN_15, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_1, GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_2, GPIO_PIN_SET);
HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_3);
HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_1);
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
// Scan_Keys();
calculate_speed_1();
calculate_speed_2();
// OLED_display_dat();
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
已有以上函数,如果要实现STM32根据8路灰度传感器的反馈结果来调整电机目标速度的功能,以上代码改如何修改?(8路灰度传感器的灰度检测阈值要求可以通过函数修改,以上程序本来的原本的功能不能变)