**FPGA** based **Field Oriented Control (FOC)** for driving **Permanent Magnet Synchronous Motors (PMSM)** or **Brushless DC Motors (BLDC)**
**FOC** puts forward certain requirements on sensor sampling rate and processor computing. Using an FPGA-based FOC can achieve better real-time performance and is more convenient for **multi-channel expansion** and **multi-channel feedback**.
This repository implements FOC based on a **angle sensor** (such as a magnetic encoder). In other word, it implements a complete **current loop** which can perform **torque control**. With this repository, you can further implement more complex motor applications using FPGA or MCU+FPGA.
|  |
| :-------------------------------: |
| **Figure1**: System diagram. |
The code in this repository has detailed comments (in Chinese). If you are familiar with Verilog but not familiar with FOC, you can quickly learn FOC by reading the code.
### Technical features
* Platform independent：Written in pure Verilog, able to run on various FPGAs such as Altera and Xilinx.
* Support **3 channels of PWM** + **1 channel of EN**: when PWM=1, the upper MOSFET turns on, and when PWM=0, the lower MOSFET turns on. When EN=0, all 6 MOSFET are turned off.
* Supports **angle sensor** and **phase current sampling ADC** with **12bit resolution**. For sensors >12bit, low-order truncation is required. For sensors <12bit,low-bitstuffingisrequired.
* Internally uses **16bit signed integer** for computation, considering that the sensors are 12bit, so 16bit computation is enough.
Figure1 is the system block diagram of the demo project, it has a simple behavior: control the current (torque) of the motor in clockwise and counterclockwise alternately. At the same time, it use the UART to print the **target value** and **actual value** of the current in order to monitor the quality of the current loop control.
All the code for this demo are in [RTL](./RTL) folder.
* FPGA board : at least 10 3.3V IO is required, connect to:
* **I2C (2*IO)** , connect to AS5600 magnetic encoder (angle sensor)
* **SPI (4*IO)** , connect to AD7928 ADC for phase current sampling.
* **PWM (3*IO)** , output 3-phase PWM to motor driving MOSFETs (typically on a motor driving board).
* **PWM_EN (1*IO)** , output a ENABLE signal to motor driving board, PWM_EN=0 is to turn off all MOSFETs. If there's no enable signal on your motor driving board, left it open.
* **UART (1*IO)** , a one-way (only sending) UART, **optionally** connected to the serial port of the computer, used to monitor the current loop.
* A PMSM or a BLDC.
* **Motor driving board**: Must support 3-phase PWM input signal (the lower arm is turned on when PWM=0, and the upper arm is turned on when PWM=1), and 3 phases **low-side resistance sampling + amplifier** must be built-in.
* **A magnetic encoder to obtain the roter angle**: This demo directly supports AS5600, which needs to be installed on the motor.
* **an ADC to sample 3-phase current**: This demo directly supports AD7928.
* I didn't find any off-the-shelf AD7928 module online, so if you want to DIY, you'll need to draw the PCB yourself.
If you have strong hardware skills, you can prepare the above hardware yourself.
But! It is recommended to directly use a **motor driver board** with an **AD7928** that I drew. Its schematic is shown in **Figure2**, and I also provide its manufacturing file [gerber_pcb_foc_shield.zip](./gerber_pcb_foc_shield .zip) , you need to take the manufacturing file to proof the PCB, and then solder it according to **Figure2**.
> Note: This board includes the **phase current sampling ADC** and **motor driveing**, but does not include the AS5600 magnetic encoder, which needs to be installed on the motor. You need to prepare it additionally.
| **Figure2**: The schematic of my motor driver board, including AD7928 ADC. |
This board is open source in LCEDA, see [oshwhub.com/wangxuan/arduino-foc-shield](https://oshwhub.com/wangxuan/arduino-foc-shield) .
The reason why this board is designed as an Arduino shield is because many FPGA development boards have an Arduino shield interface (such as DE10-Nano development board) that can be plugged directly into it. It doesn't matter if your FPGA development board does not have an Arduino shield interface, you can directly connect it with Dupont cables (short Dupont cable is recommended).
The motor driver used here is the **MP6540** chip, I have only tried to drive the low-power gimbal motor, but not the high-power motor.
You need to create an FPGA project and add all the .v source files in the [RTL](./RTL) folder (including its subdirectories) to the project. Please use fpga_top.v as the top-level file.
### clock config
There is a call to the `altpll` primitive in fpga_top.v, which convert the 50MHz clock (clk\_50m signal) input by the crystal oscillator into a 36.864MHz main clock (clk signal). The `altpll` primitive is only applicable in Altera Cyclone IV FPGA, if you are using other series of FPGAs, you need to replace it with their IP cores or primitives (e.g. Xilinx's clock wizard).
If the crystal oscillator of your development board is not 50MHz, you need to modify the PLL's configuration to ensure that the frequency of the main clock is 36.864MHz.
In fact, the frequency of the main clock can take any value less than 40MHz. main clock is to drive the FOC system, its frequency will determine the frequency of SVPWM (SVPWM frequency = clk frequency/2048), I choose 36.864MHz because it can make SVPWM frequency = 36864/2048 = 18kHz.
The reason why the frequency of main clock cannot exceed 40MHz is that adc_ad7928.v will generate the SPI clock (spi\_sck signal) by dividing the frequency by two, and the ADC7928 chip requires that the SPI clock cannot exceed 20MHz.
### pin constraints
The IO connection of fpga_top.v is as follows:
*`clk_50m` : connect to the crystal oscillator on FPGA board.
*`i2c_scl`, `i2c_sda` : connect to AS5600's I2C.
*`spi_ss`, `spi_sck`, `spi_mosi`, `spi_miso` : connect to AD7928's SPI.
*`pwm_a`, `pwm_b`, `pwm_c` : connect to 3-phase PWM of the motor driving board.
*`pwm_en` : connect to the ENABLE signal of the motor driving board.
* If there's no ENABLE signal, left it open.
* If there are 3 ENABLE signals for every 3 phases, then 1-to-3 connect is recommended.
*`uart_tx` : **optionally** connect to the Serial Port of computer (e.g., a UART-to-USB module).
| `INIT_CYCLES` | 1\~4294967293 | Determines how many clock (clk) cycles the initialization step takes. The value should not be too small, as there should be enough time for the rotor to return to electrical angle =0. For example, when the clk frequency is 36.864MHz, you can take `INIT_CYCLES=16777216`, then the initialization time is `16777216/36864000=0.45` seconds. |
| `ANGLE_INV` | 0 or 1 | If the angle sensor is not installed reversely (the rotation direction of phase A → phase B → phase C → phase A is the same as the increasing direction of the reading value of the angle sensor), this parameter should be set to 0. If the angle sensor is installed reversely, this parameter should be set to 1. |
| `POLE_PAIR` | 1\~255 | The number of pole pairs of the motor, which is determined according to the motor model (note: electrical angle ψ = number of pole pairs \* mechanical angle φ). |
| `MAX_AMP` | 1\~511 | The maximum duty of SVPWM, the smaller the value, the smaller torque the motor can achieve. But considering that we use the lower arm resistance sampling method, the value should not be too large to ensure that the lower arms have sufficient time for ADC sampling. In this example, the default value of `9'd384` is sufficient. |
| `SAMPLE_DELAY` | 0\~511 | Sampling delay, considering that the 3-phase drive MOSFET needs a certain time from the start of conduction to the current stabilization, so a certain delay is required from the conduction of the three lower arms to the ADC sampling time. This parameter determines how many clock cycles this delay is, and when the delay is over, the module generates a high-level pulse on the `sn_adc` signal, indicating that the external ADC is "ready to sample". In this example, the default value of `9'd120` is sufficient. |
In addition, foc_top module has two input ports `Kp` and `Ki` , they are parameters for PID controller. They will affect the quality of control. They can be adjusted at runtime, but you can also input fixed constants for them.
```verilog
// ports of foc_top.v
input wire [30:0] Kp,
input wire [30:0] Ki,
```
After tuning the parameters, you can compile and program to the FPGA. You should be able to see the motor spining clockwize and anticlockwize alternately.
Connect the `uart_tx` signal to the computer through **UART to USB module** (such as CP2102, CH340 module), you can use **Serial Assistant**, **Putty**, **HyperTerminal**, **minicom** or other serial port software to monitor the controll effect of the current loop.
> Note : UART config is 115200,8,n,1
The information printed by the serial port is shown as follow. The first to fourth columns are: **actual value of d-axis current**, **target value of d-axis current**, **actual value of q-axis current**, **target value of q-axis current **. It can be seen that even if the target value suddenly changes from +200 to -200, the actual value can follow the target value, indicating that the PID control of the current loop is available.
In addition, you can use the **serial plotter** of **Arduino IDE** to display the current following curve in real time. Go to [Arduino official website](https://www.arduino.cc/en/software) to download **Arduino IDE**, open it after installation, select the correct COM port in "**Tools→Port**", then Click "**Tools→Serial Port Plotter**". It will automatically receive the serial port and use the above 4 columns of data to draw real-time curves.
**Figure3** is the current following curves that I obtain. The blue curve is the data in the first column (the actual value of the d-axis current); the red curve is the data in the second column (the target value of the d-axis current); the green curve is the data in the third column (the actual value of the q-axis current); the yellow curve is the 4th column data (target value of q-axis current). It can be seen that the actual value can follow the target value.
The following table lists all the **Verilog** code files in this project, which are in the [RTL](./RTL) folder. Combined with **Figure1**, you can see the role of each module.
| uart_monitor.v | UART sender. | Can be removed if not used. |
| i2c_register_read.v | Read AS5600 magnetic encoder. | |
| adc_ad7928.v | Read AD7928 ADC. | |
| foc_top.v | FOC+SVPWM, that is, the blue part in **Figure1**. | Fixed function, generally do not need to be changed |
| clark_tr.v | Clark transform. | Fixed function, generally do not need to be changed |
| park_tr.v | Park transform. | Fixed function, generally do not need to be changed |
| sincos.v | Sine/Cosine calculator, called by park_tr.v | Fixed function, generally do not need to be changed |
| pi_controller.v | PI controller (PID without D). | Fixed function, generally do not need to be changed |
| cartesian2polar.v | Cartesian coordinate system to polar coordinate system. | Fixed function, generally do not need to be changed |
| svpwm.v | SVPWM 调制器 | Fixed function, generally do not need to be changed |
| hold_detect.v | When three lower bridge arms are all turned on, pulse the sn_adc signal after a delay, indicating that the ADC can start sampling. | Fixed function, generally do not need to be changed |
|  |
| :-------------------------------: |
| **Figure1**: System diagram. |
**Figure1** shows the hierarchy of these modules, I have fully considered the rationality of encapsulation and code reuse when designing this module hierarchy:
* The **pink** part are the sensor controllers in the FPGA, which is **hardware related logic**. If the angle sensor and ADC model change, this part of the code needs to be rewritten.
* The **blue** part is the fixed algorithm of FOC, it is **hardware independent logic**, generally does not need to be modified, it is the core code of this library!
* The **yellow** part is the **user-defined logic** in the FPGA, the user can modify the user behavior to realize various motor applications. Or modify `uart_monitor` to monitor other variables.
* The **orange** part is the hardware circuit outside the FPGA, that is, the motor, the motor driver board, and the angle sensor.
In addition, except for the `altpll` primitives in fpga_top.v, all code in this library is written in pure RTL and can be easily ported to FPGAs from other vendors (Xilinx, Lattice, etc.).
| tb_clark_park_tr.v | Testbench for clark_tr.v (clark transform) and park_tr.v (park transform). |
| tb_clark_park_tr_run_iverilog.bat | Command script to run tb_clark_park_tr.v with iverilog. |
| tb_svpwm.v | Testbench for cartesian2polar.v and svpwm.v. |
| tb_svpwm_run_iverilog.bat | Command script to run tb_svpwm.v with iverilog. |
Before using iverilog simulation, you need to install iverilog , see: [iverilog_usage](https://github.com/WangXuan95/WangXuan95/blob/main/iverilog_usage/iverilog_usage.md)
Let's run a simulation of the clark transform and park transform first.
Directly double-click tb_clark_park_tr_run_iverilog.bat to run the simulation, the waveform file dump.vcd will be generated after running. Use gtkwave to open dump.vcd. After importing signals, you can see the waveform as **Figure4**. You need to change these signals to the form of analog display:
- For the signal declared as `signed` in the Verilog code, you need to right-click "signal→Data Format→Signed Decimal".
- Right-click the "signal→Data Format→ Analog→Step" to turn it into the form of analog display.
The simulation waveform in **Figure4** shows that:
- theta θ is an increasmental angle (0→2π→0→2π→...)
- ia, ib, ic are sine waves generated by θ, ia, ib, ic form a 3-phase sine wave.
- Transform ia, ib, ic into ialpha (iα), ibeta (iβ) using clark transform to get a pair of quadrature sine waves (phase difference is π/2).
- Use park transformation to convert (iα, iβ) to the stator coordinate system to get fixed values id and iq.
Now run the simulation of cartesian2polar (cartesian to polar) and svpwm.
Directly double-click tb_svpwm_run_iverilog.bat to run the simulation, and the waveform file dump.vcd will be generated after running. Please open dump.vcd with gtkwave, you can see the waveform as **Figure5**.
Note: The three signals `pwma_duty`, `pwmb_duty`, and `pwmc_duty` are not in the top level. You can find them in the svpwm module.
Interpretation of **Figure5** waveform:
- theta θ is an increasmental angle (0→2π→0→2π→...)
- x and y are quadrature sine waves generated with θ (in other words, y is a sine wave and x is a cosine wave).
- Treat (x,y) as a Cartesian coordinate value, then cartesian2polar converts it to a polar coordinate system (rho, phi)=(ρ, φ).
- Sending (ρ, φ) to svpwm, it will generate 3 saddle waves: `pwma_duty`, `pwmb_duty`, and `pwmc_duty`. If you are familiar with the principle of seven-segment SVPWM, you should know why it is a saddle wave, and I will not introduce it here.
-`pwma_duty`, `pwmb_duty`, `pwmc_duty` determine the duty cycle of `pwm_a`, `pwm_b`, `pwm_c` respectively. You can zoom in on the waveform to verify this, see **Figure6**.
* **问**：**进行串行采样的时候不会出现这样的问题：第一个时钟周期采到的是A相的电流，第2个时钟周期采的是B相，第3个时钟周期采到的是C相，而我们知道相电流是正弦变化的，这三个时钟周期采的A B C 三相电流并不是同一时刻的相电流，因此这将产生误差，而时钟周期是很短的，是不是产生的误差几乎可以忽略不计呢？**
* [1] [Sensorless FOC for PMSM](https://www.microchip.com/stellent/groups/SiteComm_sg/documents/Training_Tutorials/en532365.pdf), MicroChip.
* [2] [Current sensing in BLDC motor application](https://www.st.com/resource/en/application_note/dm00666970-current-sensing-in-bldc-motor-application-stmicroelectronics.pdf), ST.
* [3] [Center-Aligned SVPWM Realization](https://www.ti.com/lit/an/sprabs6/sprabs6.pdf), TI
* [4] [MP6540, 3-phase, brushless DC motor drivers](https://www.monolithicpower.com/en/mp6540-mp6540a.html), MPS.
* [5] [AD7928, 8-Channel, 1 MSPS, 12-Bit ADC](https://www.analog.com/en/products/ad7928.html), Analog Devices.
