• FPGA实现电机转速PID控制

             通过纯RTL实现电机转速PID控制，包括电机编码器值读取，电机速度、正反转控制，PID算法，卡尔曼滤波，最终实现对电机速度进行控制，使其能够渐近设定的编码器目标值。

    一、设计思路

            前面通过SOPC之NIOS Ⅱ实现电机转速PID控制(调用中断函数)对电机实现了PID控制，然后就可以按照其设计方式将上层的C语言实现的PID控制部分等全部转换成Verilog代码，最终实现纯RTL进行PID控制。
            在前文中，电机PWM控制，电机方向和编码器值的获取，卡尔曼滤波是通过Verilog语言编写，而电机速度控制、PID控制是通过Nios Ⅱ系统中的软件部分实现的，因此需要编写电机速度模块，实现对电机PWM控制模块传入速度信息；编写PID控制模块，实现对电机速度模块速度的校正。
            Nios Ⅱ中采用Avalon总线对各个底层Verilog代码进行读取和写入数据，因此也要对电机控制，电机方向和编码器值获取相关代码进行修改。
            按照思路画出大概框图如下：

    二、PWM控制模块

    在前文PWM模块中由于需要Avalon总线的控制，因此有片选信号、片选地址、读写地址等变量，而转为纯RTL后只需要输入方向以及PWM值就可以，因此需要对前文代码进行修改

    1. reg motor_movement;         // 电机运动，1为开始、0为停止
    2. reg motor_direction;        // 电机转向，1为向前、0为向后
    3. reg motor_fast_decay;       // 电机减速，1为快制动、0为慢制动
    4.  
    5. always @(posedge clock or negedge reset_n)
    6. begin
    7.     if (~reset_n)
    8.     begin
    9.         // PWM
    10.         high_dur <= 0;
    11.         total_dur <= 0;
    12.         
    13.         // MOTOR
    14.         motor_movement <= 1'b0;
    15.         motor_direction <= 1'b1;
    16.         motor_fast_decay <= 1'b1;
    17.     end
    18.     else if (en)
    19.     begin
    20.         total_dur <= 32'd2500;
    21.         high_dur  <= speeddata;
    22.         {motor_fast_decay, motor_direction, motor_movement} <= {1'b1,direction,1'b1};
    23.     end
    24.  
    25. // 方向控制
    26. always @(*)
    27. begin
    28.     if (motor_fast_decay)
    29.     begin  
    30.         if (motor_movement)
    31.         begin
    32.             if (motor_direction)
    33.                 {DC_Motor_IN2, DC_Motor_IN1, PWM} <= {1'b1, 1'b0, PWM_OUT};
    34.             else
    35.                 {DC_Motor_IN2, DC_Motor_IN1, PWM} <= {1'b0, 1'b1, PWM_OUT};
    36.         end
    37.         else
    38.             {DC_Motor_IN2, DC_Motor_IN1, PWM} <= {1'b1, 1'b1, 1'b0};
    39.     end
    40.  
    41. // PWM 转速控制
    42. reg             PWM_OUT;
    43. reg     [31:0] total_dur;       // 总持续时间
    44. reg     [31:0] high_dur;        // 高位时间，决定电机转速，控制 PWM 占空比，值越高，占空比越大，转速越快
    45. reg     [31:0] tick;            // 计数器
    46.  
    47. always @(posedge clock or negedge reset_n)
    48. begin
    49.     if (~reset_n)
    50.     begin
    51.         tick <= 1;
    52.     end
    53.     else if (tick >= total_dur)
    54.     begin
    55.         tick <= 1;
    56.     end
    57.     else
    58.         tick <= tick + 1;
    59. end
    60.  
    61. always @(posedge clock)
    62. begin
    63.     PWM_OUT <= (tick <= high_dur) ? 1'b1 : 1'b0;
    64. end

    三、速度控制模块

    速度控制模块的主要任务就是将PID模块传入的速度信息，转换为PWM值传入PWM控制模块，并根据速度的正负值计算电机的正转反转

    1.  //速度控制逻辑
    2.     always @(posedge clk or negedge reset_n) begin
    3.         if (~reset_n) begin
    4.             SpeedParam <= 32'd0;
    5.             direction_reg <= 0;
    6.         end else begin
    7.             //计算PWM参数  CYCLE_WIDTH_MINI = 32'd50;CYCLE_WIDTH_MAX  = 32'd2500;
    8.             if (Speed_in > 32'd0) begin
    9.                 SpeedParam <= CYCLE_WIDTH_MINI + (Speed_in * (CYCLE_WIDTH_MAX - CYCLE_WIDTH_MINI) / 32'd100);
    10. 		    end else if(Speed_in < 32'd0) begin
    11. 				SpeedParam <= CYCLE_WIDTH_MINI + ((-Speed_in) * (CYCLE_WIDTH_MAX - CYCLE_WIDTH_MINI) / 32'd100);
    12.             end else begin
    13.                 SpeedParam <= 32'd0;
    14.             end
    15.  
    16.             //设置电机的转向
    17.             direction_reg <= Speed_in[31] ? 0 : 1;
    18.         end
    19.     end

    四、PID控制模块  

    首先要将PID参数中的小数进行缩放转为定点数；
    然后因为用了50MHz的时钟，时钟周期是10ns，可能导致KP、KI、KD算不完，因此将其进行分频为25MHz；
    然后就是对 KP、KI、KD进行计算，输出PID结果，即电机速度；
    最后为了防止电机速度和累积误差过大，对其进行限幅。

    1.     // 将Kp, Ki, Kd转化为定点数表示
    2. 	parameter KP = 32'd60; 			// 0.06
    3.     parameter KI = 32'd1; 				// 0.001
    4.     parameter KD = 32'd3400; 			// 3.4
    5. 	parameter SCALE_FACTOR = 1000;  //缩放因子
    6. 	 
    7. 	reg signed [31:0] error;
    8. 	reg signed [31:0] prev_error;
    9.     reg signed [31:0] integral;
    10.     reg signed [31:0] speed;
    11. 	reg signed [31:0] controlOutput;
    12. 	 
    13. 	reg signed [31:0] p;
    14. 	reg signed [31:0] i;
    15. 	reg signed [31:0] d;
    16. 	 
    17. 	wire signed [31:0] integral_next = integral + error;
    18. 	 
    19. 	reg clk_25m;   
    20. 	always @(posedge clk or negedge reset_n) begin
    21. 		if (~reset_n) begin
    22. 			clk_25m <= 0;
    23. 		end else begin
    24. 			clk_25m <= ~clk_25m;
    25. 		end
    26. 	end
    27.  
    28.     always @(posedge clk_25m or negedge reset_n) begin
    29.         if (~reset_n) begin
    30.             error 	  <= 0;
    31.             integral   <= 0;
    32. 				speed 	  <= 0;
    33.         end else begin
    34. 				
    35. 			error <= targetDistance - currentDistance;
    36.  
    37. 			// Calculate control output
    38.             p <= error * KP;
    39.             i <= integral * KI;
    40.             d <= (error - prev_error) * KD;
    41. 			controlOutput <= (p + i + d) / SCALE_FACTOR;
    42. 				
    43.             // 将控制输出限制在电机速度范围内
    44.             //speed <= initialSpeed + controlOutput;
    45. 			if(controlOutput > $signed(32'd100)) begin
    46. 				speed <= $signed(32'd100);
    47. 			end else if(controlOutput < $signed(-32'd100)) begin
    48. 				speed <= $signed(-32'd100);
    49. 			end else begin
    50. 				speed <= controlOutput;
    51. 			end
    52. 				
    53. 			//integral <= integrallimit + error;
    54. 			if(integral_next >= $signed(32'd800)) begin
    55. 				integral <= $signed(32'd800);
    56. 			end else if(integral_next <= $signed(-32'd800)) begin
    57. 				integral <= $signed(-32'd800);
    58. 			end else begin
    59. 				integral <= integral_next;
    60. 			end
    61.         end
    62.     end
    63. 	
    64. 	// 更新下次迭代的前一次误差和积分
    65. 	always @(posedge clk_25m or negedge reset_n) begin
    66. 	    if(~reset_n) begin
    67. 		    prev_error <= 0;
    68.         end else if(error!= prev_error) begin
    69. 			prev_error <= error;
    70. 		end else begin
    71. 		     prev_error <= prev_error;
    72. 		end
    73. 	end
    74.  
    75. 	assign speedout = speed;

    五、电机方向和编码器值的获取

    电机编码器值要根据电机方向进行自增和自减，因此要先通过AB方波确认电机方向

    1. reg  DO_PULSE;                      //用于存储输出的电机脉冲信号
    2. wire PULSE_XOR;                     //用于存储PHASE_A和PHASE_B进行异或结果
    3. reg  PULSE_XOR_PREVIOUS;            //上一次的PULSE_XOR值
    4. reg  DIRECTION;                     //用于存储电机方向信号
    5. reg  DIRECT_PATCH;                  //用于存储DIRECT异或PHASE_A后取反的结果
    6.  
    7. //解码方向信号
    8. always @(posedge DI_PHASE_A) DIRECTION <= DI_PHASE_B;                    //当有DI_PHASE_A的上升沿，将DI_PHASE_B的值赋给DIRECTION  
    9. always @(posedge DI_PHASE_B) DIRECT_PATCH <= ~(DIRECTION ^ DI_PHASE_A);  //当有DI_PHASE_B的上升沿，将DIRECT和DI_PHASE_A进行异或后取反赋值给DIRECT_PATCH 
    10. assign DO_DIRECT = DIRECTION | DIRECT_PATCH;                             //将DIRECTION和DIRECT_PATCH进行与运算 
    11.  
    12. //解码脉冲信号
    13. assign PULSE_XOR = DI_PHASE_A ^ DI_PHASE_B;                         
    14. always @(posedge DI_SYSCLK) 
    15. begin
    16.     if(PULSE_XOR != PULSE_XOR_PREVIOUS)                             
    17.     begin                                                              
    18.         DO_PULSE <= 1'b1;                                              
    19.         PULSE_XOR_PREVIOUS <= PULSE_XOR;
    20.     end
    21.     else begin                                                         
    22.         DO_PULSE <= 1'b0;
    23.     end
    24.     
    25. end

     获取电机方向后，对其进行计数，得到电机编码器的值并将其输出

    1. always @(posedge clock or negedge reset_n)
    2. begin
    3.     if(~reset_n) begin
    4.         counter_enable <= 0;
    5.     end
    6.     else if (counter_enable) begin
    7.         read_data <= pulse_counter;
    8.     end  
    9. end
    10.  
    11. always @(posedge clock)
    12. begin
    13.     if(DO_PULSE ) begin
    14.         if(motor_direction) begin                         //如果电机正转  
    15.             if(pulse_counter < $signed(32'h8000))
    16.                 pulse_counter <= pulse_counter + 1;      //counter随电机传回的脉冲数累加   
    17.         end
    18.         else if(!motor_direction) begin                  //如果电机反转
    19.              if(pulse_counter > $signed(-32'h8000))
    20.                 pulse_counter <= pulse_counter - 1;      //counter随着电机传回的脉冲数递减    
    21.         end
    22.         else
    23.             pulse_counter <= 0;                                
    24.     end
    25. end

    七、实验效果

    整体的系统框图如下所示，左右两个电机的方波经过卡尔曼滤波后输入到motor_measure中，先获取其电机方向，然后对电机编码器进行计数并将左右两电机的编码器数值取平均值输入到PID控制模块中，与ISSP输入的目标编码器值进行计算出速度信息，将速度信息输入到set_speed中计算方向和PWM参数，输入到PWM控制模块进行电机控制

    通过signal tap和ISSP联合抓波形，得出来的效果还是不错的，会有一点点超调量 

  • 相关阅读:
    js 代码中的 “use strict“； 是什么意思 ?
    MySQL数据库基础
    settings全局配置和resultMap解决处理字段名和属性名不一致问题
    SpringMVC（一）
    三相PWM整流器滞环电流控制MATLAB仿真模型
    Python描述 LeetCode 剑指 Offer II 091. 粉刷房子
    ClickHouse表引擎
    R语言CalibrationCurves包绘制带可信区间的校准曲线
    PriorityQueue 源码解析（JDK1.8）
    Dubbo笔记
  • 原文地址：https://blog.csdn.net/STATEABC/article/details/132665803