旋转倒立摆stm32HAL库（pid算法，单通道adc，直流减速电机带编码器，文末有工程代码）

1.硬件部分

先给大家看看硬件部分：先是一个直流减速电机，一定要买带编码器的直流减速电机，然后是角位移传感器，就是图中黄色的，来识别摆杆的角位移，然后一个l298n，tb6612也行，两者相差不大。板子随意，没特别要求，大体的结构如下图（我主要负责软件，硬件大家可以去了解一下其他大佬的）

2.软件部分

我们需要采取两个环才能更好的控制倒立摆，一个是角位移传感器提供的角度环，一个是编码器的位置环。即要使用连个pid，假如只用角度环，那摆杆将左右摇摆，并且当偏转角较大时会一直向一个方向偏，从而无法保持平衡，也就是当偏转恨小的时候，我们不需要过大的pid控制，可以使用位置环，即两个不同的pid。

1.adc采集角位移数据：

先配置adc，因为只有一个脚，所以我们只需要开启单通道采集就行 

我使用了一个定时器中断：间隔是5ms这里和大家说一下psc和rcc的配置关系：

计算赫兹：72*10^6除以(psc+1)(arr+1)=hz  时间分之一就是赫兹‘ 

在这里我还对采集到的adc值做了归一化处理，最大限度的避免了误差的出现。 

HAL_ADCEx_Calibration_Start(&hadc1);   //校准adc

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
	if (htim->Instance == TIM1) 
	{
		count++;
		HAL_ADC_Start_DMA(&hadc1, (uint32_t *)(adc_data), 6); // 开始ADC采样
		if (adc_ready_flag)
		{
				adc_ready_flag = 0; // 复位标志位

				// 处理ADC数据
				max = adc_data[0];
				min = adc_data[0];
				sum = 0;
				for (int i = 0; i < 6; i++)
				{
						if (adc_data[i] > max) max = adc_data[i];
						if (adc_data[i] < min) min = adc_data[i];
						sum += adc_data[i];
				}
				average = (sum - min - max) / 4;
				data = (int)(100 * ((average - min) / (max - min + 1)));
		}
    Encoder = Read_Encoder();
		MotorControl();
	}

}		

2.直流减速电机的驱动

将定时器设置为编码器模式，因为减速电机有ab连个通道，通过两个口波形的不同可以判断其速度，这就是控制倒立摆的速度环。不了解的小伙伴可以去看看原理讲解http://t.csdnimg.cn/zzHwC

我将左右和控制pwm波的函数封装到一起，方便后面使用 

void left(int motor_speed)
{
  HAL_GPIO_WritePin(in1_GPIO_Port, in1_Pin, GPIO_PIN_RESET);
  HAL_GPIO_WritePin(in2_GPIO_Port, in2_Pin, GPIO_PIN_SET);
  __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, motor_speed);
}
void right(int motor_speed)
{
  HAL_GPIO_WritePin(in1_GPIO_Port, in1_Pin, GPIO_PIN_SET);
  HAL_GPIO_WritePin(in2_GPIO_Port, in2_Pin, GPIO_PIN_RESET);
	__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, motor_speed);
}

3.采集编码器值 

 采集后记得附零，以免被覆盖

HAL_TIM_Encoder_Start(&htim2,TIM_CHANNEL_ALL);//编码器
uint16_t getEncoder(void)
{
	 uint16_t Encoder_num = __HAL_TIM_GET_COUNTER (&htim2);
	 __HAL_TIM_SET_COUNTER(&htim2, 0);
	return Encoder_num;
}

现在我们两个值都采集到了，可以进行pid运算，使其保持平衡。（我这里展示的是手动起摆）只做了两天，时间太短了，零件都是之前小车上的，哭

4.控制摆杆（两环pid）

先是电机的控制代码（这里我对其作了一个限幅，以免速度过大，如果速度太小电机转不起来，可能是我参数每调好，所以我加了一个最小速度）

void control_motor(int motor_speed)
{
    int max_motor_speed = 3000; 
	  int min_motor_speed = 500;  // 设置一个最小速度阈值
    if (motor_speed > max_motor_speed)
    {
        motor_speed = max_motor_speed;
    }
    else if (motor_speed < -max_motor_speed)
    {
        motor_speed = -max_motor_speed;
    }
		else if (motor_speed > 0 && motor_speed < min_motor_speed)
        motor_speed = min_motor_speed;
    else if (motor_speed < 0 && motor_speed > -min_motor_speed)
        motor_speed = -min_motor_speed;
    
    if (motor_speed > 0)
    {
			left(motor_speed);
    }
    else if (motor_speed < 0)
    {
			right(-motor_speed);	
    }
}

代码中的2170是摆杆在平衡位置时测出来的量，大家可以根据自己的去测量，注意我们要调整角位移传感器突变的地方为下面。 

void function3(void)
{
	  int position_pwm, balance_pwm;
	  int encoder_value = Encoder;
    float current_angle = average;
//	  float target_angle = 1980; // 目标角度
    int stability_threshold = 300;
    
    if (abs(average - 2170) <= stability_threshold)
    {
				if(++Position_Target>4)  position_pwm = Position(encoder_value);Position_Target=0;   


        balance_pwm = PID(current_angle,2170);
			
        int motor_speed = balance_pwm-position_pwm ;
			  control_motor(motor_speed);
    }
    else
    {
        control_motor(0);
    }
}

下面是两个 pid，大家可以参考一下

int Position(int Encoder)
{  
	  int  Position_Zero = Encoder;//记得测量最初位置
	  float Position_KP=25,Position_KD=600;
    static float Position_PWM, Position_Bias, Position_Differential;
    static float Last_Position = 0.0; 

    float Position_Least = Encoder - Position_Zero;

    Position_Bias *= 0.8;
    Position_Bias += Position_Least * 0.2;

    Position_Differential = Position_Bias - Last_Position;
    Last_Position = Position_Bias;

    Position_PWM = Position_Bias * Position_KP + Position_Differential * Position_KD;

    return Position_PWM;
}

float PID(float average, float target_angle)
{
	
 	int result;
	float kp = 6000, ki = 500, kd = 1600;
	

	
	float err = 0, sum_err=0;
	static float last_err = 0;
	
	sum_err+=err;
	err = average - target_angle;
	if (sum_err > 1000) sum_err = 1000;  // 防止积分饱和
  if (sum_err < -1000) sum_err = -1000;
	result = (int) (kp * err + ki * sum_err - kd * (err - last_err));
	last_err = err;
	
	return result;
}

 

以上就是手动起摆的全部内容，其实还是很简单的。

后面的我没时间写了，我还写了一个模式，分别是 

 

先是我是用按键来区别不同的模式

void MotorControl(void) 
{
	switch (num) 
	{
	case 1:
       function1();
		break;
	case 2:left(0);right(0);
	  
		break;
	case 3:		function2();

		break;
	case 4:left(0);right(0);
		
		break;
	case 5:		function3();
		break;
	case 6:
		break;
	
	  default:
		break;
	}
}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) 
{
    if(GPIO_Pin == GPIO_PIN_0) 
			{
				HAL_Delay(20);
        if(HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_0) == GPIO_PIN_RESET)
				{ 
            num++;
        }
    } 
    if(GPIO_Pin == GPIO_PIN_1) 
			{
				HAL_Delay(10);
        if(HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_1) == GPIO_PIN_RESET) 
				{ 
            num--;
        }
    }
}

然后在上面的中断void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)中，我自加了一个数count，因为定时器中断的时间是5ms，这时候如果我count小于20执行左转那么就相当于延时100ms，这里不能使用延时，因为延时时他不会执行任何程序，所以你使用延时，那么它将不会动 

再次注意，你的速度不能超过你配的rcc。

void function2(void) 
{
    if (count <= 80)         left(4400); 
	  //if(count>80|count<85){left(0); right(0);}

    if (count > 85)          right(4400);
    //if(count>85|count<170){left(0); right(0);}
    if(count > 170)          count = 0;
}

 

void function1(void) 
{
    if (count <= 70)           right(4400);
	//  if(count>80|count<85){left(0); right(0);}

    if (count > 75)            left(4400); 
  //  if(count>85|count<170){left(0); right(0);}
    if(count > 145)          count = 0;
}

 

我把整个文件放在百度网盘了，需要的可以自取，记得点个关注哦。

链接：https://pan.baidu.com/s/1JmoHoLRxgOekDg1x9P-JuQ 
提取码：1234