北京营销型网站建设多少钱,南宁微网站制作,小红书推广费用一般多少,做app 的模板下载网站有哪些内容通过纯RTL实现电机转速PID控制#xff0c;包括电机编码器值读取#xff0c;电机速度、正反转控制#xff0c;PID算法#xff0c;卡尔曼滤波#xff0c;最终实现对电机速度进行控制#xff0c;使其能够渐近设定的编码器目标值。
一、设计思路 前面通过SOPC之NIOS Ⅱ实现电… 通过纯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值就可以因此需要对前文代码进行修改
reg motor_movement; // 电机运动1为开始、0为停止
reg motor_direction; // 电机转向1为向前、0为向后
reg motor_fast_decay; // 电机减速1为快制动、0为慢制动always (posedge clock or negedge reset_n)
beginif (~reset_n)begin// PWMhigh_dur 0;total_dur 0;// MOTORmotor_movement 1b0;motor_direction 1b1;motor_fast_decay 1b1;endelse if (en)begintotal_dur 32d2500;high_dur speeddata;{motor_fast_decay, motor_direction, motor_movement} {1b1,direction,1b1};end// 方向控制
always (*)
beginif (motor_fast_decay)begin if (motor_movement)beginif (motor_direction){DC_Motor_IN2, DC_Motor_IN1, PWM} {1b1, 1b0, PWM_OUT};else{DC_Motor_IN2, DC_Motor_IN1, PWM} {1b0, 1b1, PWM_OUT};endelse{DC_Motor_IN2, DC_Motor_IN1, PWM} {1b1, 1b1, 1b0};end// PWM 转速控制
reg PWM_OUT;
reg [31:0] total_dur; // 总持续时间
reg [31:0] high_dur; // 高位时间决定电机转速控制 PWM 占空比值越高占空比越大转速越快
reg [31:0] tick; // 计数器always (posedge clock or negedge reset_n)
beginif (~reset_n)begintick 1;endelse if (tick total_dur)begintick 1;endelsetick tick 1;
endalways (posedge clock)
beginPWM_OUT (tick high_dur) ? 1b1 : 1b0;
end
三、速度控制模块
速度控制模块的主要任务就是将PID模块传入的速度信息转换为PWM值传入PWM控制模块并根据速度的正负值计算电机的正转反转 //速度控制逻辑always (posedge clk or negedge reset_n) beginif (~reset_n) beginSpeedParam 32d0;direction_reg 0;end else begin//计算PWM参数 CYCLE_WIDTH_MINI 32d50;CYCLE_WIDTH_MAX 32d2500;if (Speed_in 32d0) beginSpeedParam CYCLE_WIDTH_MINI (Speed_in * (CYCLE_WIDTH_MAX - CYCLE_WIDTH_MINI) / 32d100);end else if(Speed_in 32d0) beginSpeedParam CYCLE_WIDTH_MINI ((-Speed_in) * (CYCLE_WIDTH_MAX - CYCLE_WIDTH_MINI) / 32d100);end else beginSpeedParam 32d0;end//设置电机的转向direction_reg Speed_in[31] ? 0 : 1;endend
四、PID控制模块
首先要将PID参数中的小数进行缩放转为定点数 然后因为用了50MHz的时钟时钟周期是10ns可能导致KP、KI、KD算不完因此将其进行分频为25MHz 然后就是对 KP、KI、KD进行计算输出PID结果即电机速度 最后为了防止电机速度和累积误差过大对其进行限幅。 // 将Kp, Ki, Kd转化为定点数表示parameter KP 32d60; // 0.06parameter KI 32d1; // 0.001parameter KD 32d3400; // 3.4parameter SCALE_FACTOR 1000; //缩放因子reg signed [31:0] error;reg signed [31:0] prev_error;reg signed [31:0] integral;reg signed [31:0] speed;reg signed [31:0] controlOutput;reg signed [31:0] p;reg signed [31:0] i;reg signed [31:0] d;wire signed [31:0] integral_next integral error;reg clk_25m; always (posedge clk or negedge reset_n) beginif (~reset_n) beginclk_25m 0;end else beginclk_25m ~clk_25m;endendalways (posedge clk_25m or negedge reset_n) beginif (~reset_n) beginerror 0;integral 0;speed 0;end else beginerror targetDistance - currentDistance;// Calculate control outputp error * KP;i integral * KI;d (error - prev_error) * KD;controlOutput (p i d) / SCALE_FACTOR;// 将控制输出限制在电机速度范围内//speed initialSpeed controlOutput;if(controlOutput $signed(32d100)) beginspeed $signed(32d100);end else if(controlOutput $signed(-32d100)) beginspeed $signed(-32d100);end else beginspeed controlOutput;end//integral integrallimit error;if(integral_next $signed(32d800)) beginintegral $signed(32d800);end else if(integral_next $signed(-32d800)) beginintegral $signed(-32d800);end else beginintegral integral_next;endendend// 更新下次迭代的前一次误差和积分always (posedge clk_25m or negedge reset_n) beginif(~reset_n) beginprev_error 0;end else if(error! prev_error) beginprev_error error;end else beginprev_error prev_error;endendassign speedout speed;
五、电机方向和编码器值的获取
电机编码器值要根据电机方向进行自增和自减因此要先通过AB方波确认电机方向
reg DO_PULSE; //用于存储输出的电机脉冲信号
wire PULSE_XOR; //用于存储PHASE_A和PHASE_B进行异或结果
reg PULSE_XOR_PREVIOUS; //上一次的PULSE_XOR值
reg DIRECTION; //用于存储电机方向信号
reg DIRECT_PATCH; //用于存储DIRECT异或PHASE_A后取反的结果//解码方向信号
always (posedge DI_PHASE_A) DIRECTION DI_PHASE_B; //当有DI_PHASE_A的上升沿将DI_PHASE_B的值赋给DIRECTION
always (posedge DI_PHASE_B) DIRECT_PATCH ~(DIRECTION ^ DI_PHASE_A); //当有DI_PHASE_B的上升沿将DIRECT和DI_PHASE_A进行异或后取反赋值给DIRECT_PATCH
assign DO_DIRECT DIRECTION | DIRECT_PATCH; //将DIRECTION和DIRECT_PATCH进行与运算 //解码脉冲信号
assign PULSE_XOR DI_PHASE_A ^ DI_PHASE_B;
always (posedge DI_SYSCLK)
beginif(PULSE_XOR ! PULSE_XOR_PREVIOUS) begin DO_PULSE 1b1; PULSE_XOR_PREVIOUS PULSE_XOR;endelse begin DO_PULSE 1b0;endend 获取电机方向后对其进行计数得到电机编码器的值并将其输出
always (posedge clock or negedge reset_n)
beginif(~reset_n) begincounter_enable 0;endelse if (counter_enable) beginread_data pulse_counter;end
endalways (posedge clock)
beginif(DO_PULSE ) beginif(motor_direction) begin //如果电机正转 if(pulse_counter $signed(32h8000))pulse_counter pulse_counter 1; //counter随电机传回的脉冲数累加 endelse if(!motor_direction) begin //如果电机反转if(pulse_counter $signed(-32h8000))pulse_counter pulse_counter - 1; //counter随着电机传回的脉冲数递减 endelsepulse_counter 0; end
end
七、实验效果
整体的系统框图如下所示左右两个电机的方波经过卡尔曼滤波后输入到motor_measure中先获取其电机方向然后对电机编码器进行计数并将左右两电机的编码器数值取平均值输入到PID控制模块中与ISSP输入的目标编码器值进行计算出速度信息将速度信息输入到set_speed中计算方向和PWM参数输入到PWM控制模块进行电机控制 通过signal tap和ISSP联合抓波形得出来的效果还是不错的会有一点点超调量
