通过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
endendmodule3.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 2'd0
`define REGISTER_HIGH_DUR 2'd1
`define REGISTER_CONTROL 2'd2reg 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 <= 1'b0;motor_direction <= 1'b1;motor_fast_decay <= 1'b1;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 <= {29'b0, 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} <= {1'b1, 1'b0, PWM_OUT};else{DC_Motor_IN2, DC_Motor_IN1, PWM} <= {1'b0, 1'b1, PWM_OUT};endelse{DC_Motor_IN2, DC_Motor_IN1, PWM} <= {1'b1, 1'b1, 1'b0};endelsebegin // 慢刹车if (motor_movement)beginif (motor_direction){DC_Motor_IN2, DC_Motor_IN1, PWM} <= {1'b1, 1'b0, PWM_OUT};else{DC_Motor_IN2, DC_Motor_IN1, PWM} <= {1'b0, 1'b1, PWM_OUT};endelse{DC_Motor_IN2, DC_Motor_IN1, PWM} <= {1'b0, 1'b0, 1'b0};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) ? 1'b1 : 1'b0;
endendmodule3.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 <= 1'b1; PULSE_XOR_PREVIOUS <= PULSE_XOR;endelse begin DO_PULSE <= 1'b0;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 < 16'hffff)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,I'll 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(integral>10000) integral= 10000; //避免累积误差过大if(integral<-10000) integral=-10000;Motor.SetMotorSpeed(speed);// 打印当前误差和当前速度printf("误差：%d，当前速度：%.2f\n", error, speed);usleep(1000*100);if(error==0) {printf("Enter next targetDistance");scanf("%d", &targetDistance);}//reach = true;}return 0;
}四、实验结果
电机首先快速向目标编码值转动，最后逐渐收敛，但PID太难调了，调了好久还是会出现震荡
