SOPC之NIOS Ⅱ实现电机转速PID控制(调用中断函数)

news2024/11/25 20:51:52

通过FPGA开发板上的NIOS Ⅱ搭建电机控制的硬件平台，包括电机正反转、编码器的读取，再通过软件部分实现PID算法对电机速度进行控制，使其能够渐近设定的编码器目标值。

一、问题与改进

SOPC之NIOS Ⅱ实现电机转速PID控制_STATEABC的博客-CSDN博客

在前面用PID实现了基于NIOS Ⅱ的电机转动控制，但是由于用的usleep()函数精度不够，所以会导致有时读取的编码器值不准确，最终发生PID产生振荡现象。

因此采用中断函数的方式，每10ms调用一次中断服务函数，再中断函数中实现PID控制，从而达到更大的精确度，减少振荡。

二、硬件设计

硬件设计同之前不变

三、软件设计

3.1 中断函数初始化

 void MPU_INT_INIT(void)
  {
  	 IOWR_ALTERA_AVALON_PIO_EDGE_CAP(MPU_INT_BASE,0x00);
  #ifdef ALT_ENHANCED_INTERRUPT_API_PRESENT
       if ((alt_ic_isr_register(MPU_INT_IRQ_INTERRUPT_CONTROLLER_ID,
                                     MPU_INT_IRQ,
                                        MPU_INT_ISR,
                                 NULL,
                                 NULL
                                 )!= 0))

  #else
      if((alt_irq_register(MPU_INT_IRQ,NULL, MPU_INT_ISR  )!= 0))
  #endif
      	{
  			  printf("register irt failed\r\n");
  		  }
  		IOWR_ALTERA_AVALON_PIO_IRQ_MASK(MPU_INT_BASE,0x01);
  }

3.2 中断函数

#ifdef ALT_ENHANCED_INTERRUPT_API_PRESENT //nios2 91 edition or later
void MPU_INT_ISR(void *contex)
#else //before nios2 91 edition
void MPU_INT_ISR(void * contex, alt_u32 id)
#endif
{
	if(!IORD_ALTERA_AVALON_PIO_EDGE_CAP(MPU_INT_BASE)){
		return;
	}else
	{
		IOWR_ALTERA_AVALON_PIO_EDGE_CAP(MPU_INT_BASE,0x00);
		IOWR_ALTERA_AVALON_PIO_IRQ_MASK(MPU_INT_BASE,0x00);

		// 测量当前编码器计数
		//Car.Get_Encode();
		//int currentCountsL = Car.Encode_L;
		//int currentCountsR = abs(Car.Encode_R);

		// 执行插值步骤
		for (int currentstep = 0; currentstep < step; currentstep++) {

			Car.Get_Encode();
			int currentCountsL = Car.Encode_L;

			interpolatedTarget= currentCountsL + (targetDistance - currentCountsL) * currentstep / step;
/*			if(interpolatedTarget<=currentCountsL){
				interpolatedTarget= currentCountsL + (targetDistance - currentCountsL) * currentstep / step;
			}*/

		    // 计算误差
		    error = interpolatedTarget - currentCountsL;

		    // 计算 PID 控制输出
		    float controlOutput = calculatePID(error, integral, prev_error);

		    // 将控制输出限制在电机速度范围内
		    float speed = initialSpeed + controlOutput;
		    speed = fmaxf(-100, fminf(speed, 100));

		    // 更新下次迭代的前一次误差和积分
		    prev_error = error;
		    integral  += error;
		    if (integral >  8000) integral =  8000;
		    if (integral < -8000) integral = -8000;

		    Car.SetSpeed(speed, 0);
		}
	}
	IOWR_ALTERA_AVALON_PIO_IRQ_MASK(MPU_INT_BASE,0x01);
}

3.3 主程序

int main()
{
  	Car.Stop();
  	Car.Start();

  	MPU_INT_INIT();
  	while(1){
  		printf("Enter targetDistance");
  		scanf("%d", &targetDistance);
  	}

   return 0;
}