荣耀手机商城官方网站登录入口,爱用网站建设,个人网站赏析,阿里云服务器免费试用通过FPGA开发板上的NIOS Ⅱ搭建电机控制的硬件平台#xff0c;包括电机正反转、编码器的读取#xff0c;再通过软件部分实现PID算法对电机速度进行控制#xff0c;使其能够渐近设定的编码器目标值。
一、PID算法 PID算法#xff08;Proportional-Integral-Derivative Algo… 通过FPGA开发板上的NIOS Ⅱ搭建电机控制的硬件平台包括电机正反转、编码器的读取再通过软件部分实现PID算法对电机速度进行控制使其能够渐近设定的编码器目标值。
一、PID算法 PID算法Proportional-Integral-Derivative Algorithm是一种经典的控制算法用于实现闭环控制系统中的自动控制旨在使被控制系统的输出尽可能接近期望值。 PID算法由三个部分组成比例Proportional、积分Integral和微分Derivative。这三个部分分别对应了误差的当前值、累积值和变化率。PID算法根据这些部分的加权和来计算控制输出以实现对系统的稳定、快速、精确的控制。 比例(P)部分比例控制是根据当前的误差值来调整控制输出。如果误差较大比例部分的输出也会较大从而更快地减小误差。这有助于系统快速接近期望值但可能导致震荡和过冲。积分(I)部分积分控制用于消除系统存在的稳态误差。它考虑误差的累积值如果误差持续存在积分部分的输出会逐渐增加从而逐步减小稳态误差。然而过大的积分作用可能导致系统响应过于缓慢或产生震荡。微分(D)部分微分控制用于预测误差的未来变化趋势。通过考虑误差的变化率微分部分可以抑制系统的过冲和震荡。但过大的微分作用可能引起噪音的放大。 PID算法的参数调节是一个复杂的过程需要根据被控制系统的特性和性能要求进行调试。不同的应用和系统可能需要不同的PID参数配置以达到最佳的控制效果。
PID算法详细内容和调试方法可参考PID参数解析调参经验笔记经验法_pid调参_Xuan-ZY的博客-CSDN博客
二、硬件设计
硬件部分主要由两部分组成卡尔曼滤波和NIOS Ⅱ系统 2.1 卡尔曼滤波
卡尔曼滤波主要是为了对霍尔传感器输出的方波进行滤波操作
reg [15:0] filterClockDivider; // 过滤器时钟分频器
reg filterClock; // 过滤器时钟信号// 时钟设置
parameter ClockFrequency 50000000; // 时钟频率50MHz
parameter FilterFrequency 15000; // 滤波器频率15KHz/ 过滤器时钟
always (posedge Clock or negedge Reset)
beginif (!Reset)beginfilterClock 0; // 复位时过滤器时钟为低电平filterClockDivider 0; // 复位时分频器清零endelsebeginif (filterClockDivider (ClockFrequency / FilterFrequency / 8))filterClockDivider filterClockDivider 1; // 分频计数增加elsebeginfilterClockDivider 0; // 分频计数清零filterClock ~filterClock; // 过滤器时钟翻转endend
endalways (posedge filterClock or negedge Reset)
beginif (!Reset)beginOutput_A 0; // 复位时输出信号 A 为低电平endelsebeginif (Input_A)Output_A 1; // 如果输入信号 A 为高输出信号 A 为高elseOutput_A 0; // 否则输出信号 A 为低end
endalways (posedge filterClock or negedge Reset)
beginif (!Reset)beginOutput_B 0; // 复位时输出信号 B 为低电平endelsebeginif (Input_B)Output_B 1; // 如果输入信号 B 为高输出信号 B 为高elseOutput_B 0; // 否则输出信号 B 为低end
endendmodule
3.1 NIOS Ⅱ系统
NIOS Ⅱ中包含时钟CLK、锁相环PLL、NIOS Ⅱ软核处理器、片上存储onchip_ram、System ID、串行通信jtag_uart以及自定义组件电机控制Motor_PWM、编码器测量Motor_measure 3.1.1 电机控制
一般的FPGA是无法驱动电机的因此需要电机驱动芯片控制DC电机与FPGA相连接的控制信号有IN1/IN2/PWM通过IN1/IN2去控制电机的方向与停止通过PWM去控制电机的转速
define REGISTER_TOTAL_DUR 2d0
define REGISTER_HIGH_DUR 2d1
define REGISTER_CONTROL 2d2reg 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 (select_cs (select_address REGISTER_CONTROL))beginif (select_write){motor_fast_decay, motor_direction, motor_movement} select_writedata[2:0];else if (select_read)select_readdata {29b0, motor_fast_decay, motor_direction, motor_movement};endelse if (select_cs select_write)beginif (select_address REGISTER_TOTAL_DUR)total_dur select_writedata;else if (select_address REGISTER_HIGH_DUR)high_dur select_writedata;endelse if (select_cs select_read)beginif (select_address REGISTER_TOTAL_DUR)select_readdata total_dur;else if (select_address REGISTER_HIGH_DUR)select_readdata high_dur;end
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};endelsebegin // 慢刹车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} {1b0, 1b0, 1b0};end
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;
endendmodule 3.1.2 电机监测 电机转动带动磁盘经过霍尔传感器磁力的变化让霍尔效应传感器产生霍尔效应电压经过数字电路处理产生方波两个位置不同的霍尔效应传感器输出两个相位不同的方波(PhaseA和Phase B)。磁盘在转动时先被感应的传感器会先输出方波另一个传感器输出会有延迟所以两个方波的相位有所不同。由此可以通过方波相位领先计算电机的方向。根据输出的脉冲数可以计算电机转速。
首先计算电机的方向
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) //当复位有效将counter_threshold和counter_enable置为0begincounter_threshold 0;counter_enable 0;endelse if (select_chip_enable select_write) //当select_chip_enable和select_write有效即写有效beginif(select_register_address COUNTER_ENABLE)counter_enable select_write_data;end else if(select_chip_enable select_read)begin //当读有效就读取当前counter数值if(select_register_address COUNTER_READ)select_read_data pulse_counter;end
endalways (posedge clock)
beginif(select_chip_enable select_write select_register_address COUNTER_WRITE)pulse_counter select_write_data[15:0];else if(counter_enable motor_pulse) //当计数使能和电机脉冲同时有效beginif(motor_direction) //如果电机正转 beginif(pulse_counter 16hffff)pulse_counter pulse_counter 1; //counter随电机传回的脉冲数累加 endelse if(!motor_direction) //如果电机反转beginif(pulse_counter 0)pulse_counter pulse_counter - 1; //counter随着电机传回的脉冲数递减 endelsepulse_counter 0; end
end 三、软件设计
3.1 Motor控制
电机控制部分由Motor.h和Motor.cpp组成对相关函数进行声明和定义
首先在构造函数中传入电机的地址和测量寄存器地址
Motor::Motor(int Add,int MeasureAdd):motor_BaseAddress(Add),measure_Address(MeasureAdd),CycleWidthMini(CYCLE_WIDTH_MINI), //电机的最小PWM周期宽度值CycleWidthMaxi(CYCLE_WIDTH_MAX) //电机的最大PWM周期宽度值{//将REG_TOTAL_DUR寄存器0设置为常量值CYCLE_WIDTH设置电机的初始PWM周期宽度IOWR(motor_BaseAddress, REG_TOTAL_DUR, CYCLE_WIDTH);
}
控制电机的启动、停止以及方向
//启动电机
void Motor::StartMotor(void){int currentStatus, updatedControl;currentStatus IORD(motor_BaseAddress, STATUS_REG); //从STATUS_REG寄存器读取当前状态updatedControl currentStatus | MOTOR_RUN_FLAG; //设置MOTOR_RUN_FLAGIOWR(motor_BaseAddress, STATUS_REG, updatedControl); //然后将更新后的控制值写回寄存器IOWR(measure_Address, measure_count_enable_reg, 0x01); //将0x01写入测量设备的measure_cnt_enable_reg 寄存器来启用测量计数器
}//停止电机
void Motor::StopMotor(void){int currentStatus, updatedControl;currentStatus IORD(motor_BaseAddress, STATUS_REG); //从STATUS_REG寄存器读取当前状态updatedControl currentStatus (~MOTOR_RUN_FLAG); //清除MOTOR_RUN_FLAGIOWR(motor_BaseAddress, STATUS_REG, updatedControl); //将更新后的控制值写回寄存器IOWR(measure_Address, measure_count_enable_reg, 0x00); //通过将0写入测量设备的 measure_count_enable_reg 寄存器来禁用测量计数器
}//设置电机方向
void Motor::SetMotorDirection(bool forwardDirection){ int currentStatus, updatedControl;currentStatus IORD(motor_BaseAddress, STATUS_REG); //从STATUS_REG寄存器中读取当前状态if (forwardDirection) //根据forwardDirection参数修改方向控制updatedControl currentStatus | MOTOR_FORWARD_FLAG;elseupdatedControl currentStatus ~MOTOR_FORWARD_FLAG;IOWR(motor_BaseAddress, STATUS_REG, updatedControl); //将更新后的值写回STATUS_REG寄存器中
}设置电机转动的速度
//设置电机速度
void Motor::SetMotorSpeed(float desiredSpeed) {int speedParameter 0;if (desiredSpeed -100.0) //确保输入速度在有效范围内desiredSpeed -100;else if (desiredSpeed 100.0)desiredSpeed 100.0;if (desiredSpeed ! 0.0) { //根据输入速度计算适当的PWM值speedParameter motor_CycleWidth_Min (int)(fabs(desiredSpeed) * (float)(motor_CycleWidth_Max - motor_CycleWidth_Min) / 100.0);}IOWR(motor_BaseAddress, REG_HIGH_DURATION, speedParameter); //将计算得到的PWM值写入REG_HIGH_DURATION寄存器以控制电机速度。SetMotorDirection((desiredSpeed 0.0) ? true : false); //使用SetMotorDirection()函数设置电机的方向
}
从寄存器中读取电机编码器的值
//获取编码器计数值
signed short Motor::GetMotorCount(void) {signed short motorCount; //带符号的16位整数motorCount IORD(motor_MeasureAddress, motor_measure_count_read_reg); //从内存映射寄存器读取速度计数motorCount motorCount - 0x8000; //减去0x8000清除count最高位即最高位为1的标志位以获取实际计数值return motorCount;
}
3.2 主程序
在主程序中实现PID算法
float kp 0.02; //比例增益
float ki 0.015; //积分增益
float kd 0.35; //微分增益float calculatePID(float error, float integral, float prev_error) {float p kp * error; //偏差error 目标值 - 当前值float i ki * integral; //误差和float d kd * (error - prev_error); return p i d;
}
在主函数中实现对电机的控制使其能够渐近设定的编码器目标值
int main()
{int targetDistance; // 将此值更改为所需的目标距离scanf(%d, targetDistance);printf(Hello BAL-Car,Ill keep moving\r\n);Motor.StopMotor();Motor.StartMotor();float initialSpeed 0; // 初始速度Motor.SetSpeed(initialSpeed);// 初始化PID变量float integral 0;float prev_error 0;bool reach false;while(!reach){// 测量当前编码器计数int currentCounts Motor.GetMotorCount();printf(编码器%d\n, currentCounts);// 计算误差即目标距离与当前距离的差值int error targetDistance - currentCounts;// 计算 PID 控制输出float controlOutput calculatePID(error, integral, prev_error);// 将控制输出限制在电机速度范围内float speed initialSpeed controlOutput;speed fmaxf(-100, fminf(speed, 100));// 更新下次迭代的前一次误差和积分prev_error error;integral error;if(integral10000) integral 10000; //避免累积误差过大if(integral-10000) integral-10000;Motor.SetMotorSpeed(speed);// 打印当前误差和当前速度printf(误差%d当前速度%.2f\n, error, speed);usleep(1000*100);if(error0) {printf(Enter next targetDistance);scanf(%d, targetDistance);}//reach true;}return 0;
}
四、实验结果
电机首先快速向目标编码值转动最后逐渐收敛但PID太难调了调了好久还是会出现震荡
