397 lines
11 KiB
Verilog
397 lines
11 KiB
Verilog
/************ Introduction to PI Controllers
|
||
* The continuous form of a PI loop is
|
||
*
|
||
* A(t) = P e(t) + I∫ e(t')dt'
|
||
* where e(t) is the error (setpoint - measured), and
|
||
* the integral goes from 0 to the current time 't'.
|
||
*
|
||
* In digital systems the integral must be approximated.
|
||
* The normal way of doing this is a first-order approximation
|
||
* of the derivative of A(t).
|
||
*
|
||
* dA(t)/dt = P de(t)/dt + Ie(t)
|
||
* A(t_n) - A(t_{n-1}) ≅ P (e(t_n) - e(t_{n-1})) + Ie(t_n)Δt
|
||
* A(t_n) ≅ A(t_{n-1}) + e(t_n)(P + IΔt) - Pe(t_{n-1})
|
||
*
|
||
* Using α = P + IΔt, and denoting A(t_{n-1}) as A_p,
|
||
*
|
||
* A ≅ A_p + αe - Pe_p.
|
||
*
|
||
* The formula above is what this module implements. This way,
|
||
* the controller only has to store two values between each
|
||
* run of the loop: the previous error and the previous output.
|
||
* This also reduces the amount of (redundant) computations
|
||
* the loop must execute each iteration.
|
||
*
|
||
* Calculating α requires knowing the precise timing of each
|
||
* control loop cycle, which in turn requires knowing the
|
||
* ADC and DAC timings. This is done outside the Verilog code.
|
||
* and can be calculated from simulating one iteration of the
|
||
* control loop.
|
||
*
|
||
*************** Fixed Point Integers *************
|
||
* A regular number is stored in decimal: 123056.
|
||
* This is equal to
|
||
* 6*10^0 + 5*10^1 + 0*10^2 + 3*10^3 + 2*10^4 + 1*10^5.
|
||
* A whole binary number is only ones and zeros: 1101, and is
|
||
* equal to
|
||
* 1*2^0 + 0*2^1 + 1*2^2 + 1*2^3.
|
||
*
|
||
* Fixed-point integers shift the exponent of each number by a
|
||
* fixed amount. For instance, 123.056 is
|
||
* 6*10^-3 + 5*10^-2 + 0*10^-1 + 3*10^0 + 2*10^1 + 1*10^2.
|
||
* Similarly, the fixed point binary integer 11.01 is
|
||
* 1*2^-2 + 0*2^-1 + 1*2^0 + 1*2^1.
|
||
*
|
||
* To a computer, a whole binary number and a fixed point binary
|
||
* number are stored in exactly the same way: no decimal point
|
||
* is stored. It is only the *interpretation* of the data that
|
||
* changes.
|
||
*
|
||
* Fixed point numbers are denoted WHOLE.FRAC or [WHOLE].[FRAC],
|
||
* where WHOLE is the amount of whole number bits (including sign)
|
||
* and FRAC is the amount of fractional bits (2^-1, 2^-2, etc.).
|
||
*
|
||
* The rules for how many digits the output has given an input
|
||
* is the same for fixed point binary and regular decimals.
|
||
*
|
||
* Addition: W1.F1 + W2.F2 = [max(W1,W2)+1].[max(F1,F2)]
|
||
* Multiplication: W1.F1 * W2.F2 = [W1+W2].[F1+F2]
|
||
*
|
||
*************** Precision **************
|
||
* The control loop is designed around these values, but generally
|
||
* does not hardcode them.
|
||
*
|
||
* Since α and P are precalculated outside of the loop, their
|
||
* conversion to numbers the loop understands is done outside of
|
||
* the loop and in the kernel.
|
||
*
|
||
* The 18-bit ADC is twos-compliment, -10.24V to 10.24V,
|
||
* with 78μV per increment.
|
||
* The 20-bit DAC is twos-compliment, -10V to 10V.
|
||
*
|
||
* The `P` constant has a minimum value of 1e-7 with a precision
|
||
* of 1e-9, and a maxmimum value of 1.
|
||
*
|
||
* The `I` constant has a minimum value of 1e-4 with a precision
|
||
* of 1e-6 and a maximum value of 100.
|
||
*
|
||
* Δt is cycles/100MHz. This makes Δt at least 10 ns, with a
|
||
* maximum of 1 ms.
|
||
*
|
||
* Intermediate values are 48-bit fixed-point integers multiplied
|
||
* by the step size of the ADC. The first 18 bits are the whole
|
||
* number and sign bits. This means intermediate values correspond
|
||
* exactly to values as understood by the ADC, with extra precision.
|
||
*
|
||
* To get the normal fixed-point value of an intermediate value,
|
||
* multiply it by 78e-6. To convert a normal fixed-point integer
|
||
* to an intermediate value, multiply it by 1/78e-6. In both
|
||
* cases, the conversion constant is a normal fixed-point integer.
|
||
*
|
||
* For instance, to encode the value 78e-6 as an intermediate
|
||
* value, multiply it by 1/78e-6 to obtain 1. Thus the value
|
||
* should be stored as 1 (whole bit) followed by zeros (fractional
|
||
* bits).
|
||
*/
|
||
|
||
/*
|
||
* 0x3214*78e-6 = 0.9996 + lower order (storable as 15 whole bits)
|
||
*/
|
||
|
||
`define ERR_WID (ADC_WID + 1)
|
||
|
||
module control_loop
|
||
#(
|
||
parameter ADC_WID = 18,
|
||
/* Code assumes DAC_WID > ADC_WID. If/when this is not the
|
||
* case, truncation code must be changed.
|
||
*/
|
||
parameter DAC_WID = 24,
|
||
/* Analog Devices DACs have a register code in the upper 4 bits.
|
||
* The data follows it.
|
||
*/
|
||
parameter DAC_DATA_WID = 20,
|
||
parameter CONSTS_WID = 48, // larger than ADC_WID
|
||
parameter CONSTS_FRAC_WID = CONSTS_WID-15,
|
||
parameter DELAY_WID = 16,
|
||
/* [ERR_WID_SIZ-1:0] must be able to store
|
||
* ERR_WID (ADC_WID + 1).
|
||
*/
|
||
parameter ERR_WID_SIZ = 6,
|
||
parameter ADC_POLARITY = 1,
|
||
parameter ADC_PHASE = 0,
|
||
parameter DAC_POLARITY = 0,
|
||
parameter DAC_PHASE = 1
|
||
) (
|
||
input clk,
|
||
input arm,
|
||
|
||
output adc_sck,
|
||
input adc_in,
|
||
output adc_conv,
|
||
|
||
output dac_sck,
|
||
output dac_ss,
|
||
output dac_out,
|
||
|
||
/* Informational output.
|
||
* These registers are also used for storing information while
|
||
* the loop is running.
|
||
*/
|
||
output signed [ERR_WID-1:0] err_cur,
|
||
output signed [CONSTS_WID-1:0] adj,
|
||
|
||
input signed [ADC_WID-1:0] setpt_in,
|
||
input signed [CONSTS_WID-1:0] cl_alpha_in,
|
||
input signed [CONSTS_WID-1:0] cl_p_in,
|
||
input [DELAY_WID-1:0] delay_in
|
||
);
|
||
|
||
/* Registers used to lock in values at the start of each iteration */
|
||
reg signed [ADC_WID-1:0] setpt = 0;
|
||
reg signed [CONSTS_WID-1:0] cl_alpha_reg = 0;
|
||
reg signed [CONSTS_WID-1:0] cl_p_reg = 0;
|
||
reg [DELAY_WID-1:0] saved_delay = 0;
|
||
|
||
/* Registers for PI calculations */
|
||
reg signed [ERR_WID-1:0] err_prev = 0;
|
||
|
||
/****** State machine
|
||
*
|
||
* -> WAIT_ON_ARM -> WAIT_ON_ADC -> WAIT_ON_MUL -\
|
||
* \------\------------ WAIT_ON_DAC </
|
||
*
|
||
* The loop will stop and reset all stored data if `arm` is not 1 at
|
||
* the end of the loop.
|
||
* The loop stores all data from the input into registers at
|
||
* `WAIT_ON_ADC`, so the program can change constants and setpoints
|
||
* on the fly.
|
||
*/
|
||
|
||
localparam WAIT_ON_ARM = 0;
|
||
localparam WAIT_ON_ADC = 1;
|
||
localparam WAIT_ON_MUL = 2;
|
||
localparam WAIT_ON_DAC = 3;
|
||
localparam STATESIZ = 2;
|
||
|
||
reg [STATESIZ-1:0] state = WAIT_ON_ARM;
|
||
|
||
/***** Outline *****
|
||
* The loop will only iterate when armed. If it is running and `arm`
|
||
* is deasserted, then it will complete the iteration it is in and
|
||
* stop.
|
||
*
|
||
* At the start of each loop, the constants are read into registers,
|
||
* so the constants can change while the loop is running.
|
||
*
|
||
* First the loop reads from the ADC, and then computes the error
|
||
* from the setpoint. The setpoint is specified in the same units
|
||
* as the ADC.
|
||
*
|
||
* Afterwards the loop computes the multiplications in the PI loop.
|
||
* This changes the size of the values in the loop.
|
||
*
|
||
* Combinatorially, the new adjusted value is calculated. The original
|
||
* value has to be stored in the same width as the multiplied values.
|
||
*
|
||
* Then the value is truncated to the width of the DAC, with saturation
|
||
* if necessary, and written out to the DAC.
|
||
*
|
||
**** Precision Propogation
|
||
*
|
||
* Measured value: ADC_WID.0
|
||
* Setpoint: ADC_WID.0
|
||
* - ----------------------------|
|
||
* e: ERR_WID.0
|
||
*
|
||
* α: CONSTS_WHOLE.CONSTS_FRAC | P: CONSTS_WHOLE.CONSTS_FRAC
|
||
* e: ERR_WID.0 | e_p: ERR_WID.0
|
||
* x ----------------------------| x-----------------------------
|
||
* αe: CONSTS_WHOLE+ERR_WID.CONSTS_FRAC - Pe_p: CONSTS_WHOLE+ERR_WID.CONSTS_FRAC
|
||
* + A_p: CONSTS_WHOLE+ERR_WID.CONSTS_FRAC
|
||
* --------------------------------------------------------------
|
||
* A_p + αe - Pe_p: CONSTS_WHOLE+ERR_WID+1.CONSTS_FRAC
|
||
* --> discard fractional bits: CONSTS_WHOLE+ADC_WID+1.(DAC_DATA_WID - ADC_WID)
|
||
* --> Saturate-truncate: ADC_WID.(DAC_DATA_WID-ADC_WID)
|
||
* --> reinterpret and write into DAC: DAC_DATA_WID.0
|
||
*/
|
||
|
||
/******* Get measured value ********/
|
||
|
||
reg signed [ADC_WID-1:0] measured = 0;
|
||
reg adc_arm = 0;
|
||
wire adc_finished;
|
||
spi_master_no_write #(
|
||
.WID(ADC_WID),
|
||
.POLARITY(ADC_POLARITY),
|
||
.PHASE(ADC_PHASE),
|
||
.CYCLE_HALF_WAIT(1),
|
||
.TIMER_LEN(3)
|
||
) adc (
|
||
.clk(clk),
|
||
.from_slave(adcbuf),
|
||
.miso(adc_in),
|
||
.sck_wire(adc_sck),
|
||
.arm(adc_arm),
|
||
.finished(adc_finished)
|
||
);
|
||
|
||
/**** Calculate Error ****/
|
||
assign err_cur = measured - setpoint;
|
||
|
||
/****** Multiplication *******
|
||
* Truncation of a fixed-point integer to a smaller buffer requires
|
||
* 1) truncating higher order bits
|
||
* 2) removing lower order bits
|
||
*
|
||
* The ADC number has no fractional digits, so the fixed point output
|
||
* is [CONSTS_WHOLE + ERR_WID].CONSTS_FRAC_WID
|
||
* with total width CONSTS_WID + ERR_WID
|
||
*
|
||
* Both multipliers are armed at the same time.
|
||
* Their output wires are ANDed together so the state machine
|
||
* progresses when both are finished.
|
||
*/
|
||
|
||
localparam MUL_WHOLE_WID = CONSTS_WHOLE + ERR_WID;
|
||
localparam MUL_FRAC_WID = CONSTS_FRAC;
|
||
localparam MUL_WID = MUL_WHOLE_WID + MUL_FRAC_WID;
|
||
|
||
reg arm_mul = 0;
|
||
|
||
wire alpha_err_fin;
|
||
wire signed [MUL_WID-1:0] alpha_err;
|
||
wire p_err_prev_fin;
|
||
wire signed [MUL_WID-1:0] p_err_prev;
|
||
|
||
wire mul_finished = alpha_err_prev_fin & p_err_fin;
|
||
|
||
/* αe */
|
||
boothmul #(
|
||
.A1_LEN(CONSTS_WID),
|
||
.A2_LEN(ERR_WID),
|
||
.A2LEN_SIZ(ERR_WID_SIZ)
|
||
) boothmul_alpha_err_mul (
|
||
.clk(clk),
|
||
.arm(arm_mul),
|
||
.a1(cl_alpha_reg),
|
||
.a2(err),
|
||
.outn(alpha_err),
|
||
.fin(alpha_err_fin)
|
||
);
|
||
|
||
/* Pe_p */
|
||
boothmul #(
|
||
.A1_LEN(CONSTS_WID),
|
||
.A2_LEN(ERR_WID),
|
||
.A2LEN_SIZ(ERR_WID_SIZ)
|
||
) booth_mul_P_err_mul (
|
||
.clk(clk),
|
||
.arm(arm_mul),
|
||
.a1(cl_p_reg),
|
||
.a2(err_prev),
|
||
.outn(p_err_prev),
|
||
.fin(p_err_prev_fin)
|
||
);
|
||
|
||
/**** Subtraction after multiplication ****/
|
||
localparam SUB_WHOLE_WID = MUL_WHOLE_WID + 1;
|
||
localparam SUB_FRAC_WID = MUL_FRAC_WID;
|
||
localparam SUB_WID = SUB_WHOLE_WID + SUB_FRAC_WID;
|
||
|
||
reg signed [SUB_WID-1:0] adj_old;
|
||
wire signed [SUB_WID-1:0] newadj = adj_old + alpha_err - p_err_prev;
|
||
|
||
/**** Discard fractional bits ****
|
||
* The truncation of the subtraction result first takes off the lower
|
||
* order bits:
|
||
* [ SUB_WHOLE_WID ].[ SUB_FRAC_WID ]
|
||
* [ SUB_WHOLE_WID ].[RTRUNC_FRAC_WID]############
|
||
* (SUB_FRAC_WID - RTRUNC_FRAC_WID)^
|
||
*/
|
||
|
||
localparam RTRUNC_FRAC_WID = DAC_DATA_WID - ADC_WID;
|
||
localparam RTRUNC_WHOLE_WID = SUB_WHOLE_WID;
|
||
localparam RTRUNC_WID = RTRUNC_WHOLE_WID + RTRUNC_FRAC_WID;
|
||
|
||
wire signed rtrunc[RTRUNC_WID-1:0] =
|
||
newadj[SUB_WID-1:SUB_FRAC_WID-RTRUNC_FRAC_WID];
|
||
|
||
/**** Truncate-Saturate ****
|
||
* Truncate the result into a value acceptable to the DAC.
|
||
* [ SUB_WHOLE_WID ].[RTRUNC_FRAC_WID]############
|
||
* [ADC_WID].[DAC_DATA_WID - ADC_WID]
|
||
* reinterpreted as
|
||
* [DAC_DATA_WID].0
|
||
*/
|
||
|
||
wire signed dac_adj_val[DAC_DATA_WID-1:0];
|
||
|
||
intsat #(
|
||
.IN_LEN(RTRUNC_WID),
|
||
.LTRUNC(DAC_DATA_WID)
|
||
) (
|
||
.inp(newadj_rtrunc),
|
||
.outp(dac_adj_val)
|
||
);
|
||
|
||
/**** Write to DAC ****/
|
||
|
||
reg dac_arm = 0;
|
||
spi_master_no_read #(
|
||
.WID(DAC_WID),
|
||
.WID_LEN(5),
|
||
.CYCLE_HALF_WAIT(3),
|
||
.TIMER_LEN(3),
|
||
.POLARITY(DAC_POLARITY),
|
||
.PHASE(DAC_PHASE)
|
||
) dac (
|
||
.clk(clk),
|
||
.to_slave({4'b0010,dac_adj_val}),
|
||
.mosi(dac_out),
|
||
.sck_wire(dac_sck),
|
||
.arm(dac_arm)
|
||
);
|
||
|
||
reg [DELAY_WID-1:0] timer = 0;
|
||
|
||
always @ (posedge clk) begin
|
||
case (state)
|
||
WAIT_ON_ARM: begin
|
||
if (!arm) begin
|
||
adj_prev <= 0;
|
||
err_prev <= 0;
|
||
timer <= 0;
|
||
end else if (timer == 0) begin
|
||
saved_delay <= delay_in;
|
||
timer <= 1;
|
||
end else if (timer < saved_delay) begin
|
||
timer <= timer + 1;
|
||
setpt <= setpt_in;
|
||
cl_alpha_reg <= cl_alpha_in;
|
||
cl_p_reg <= cl_p_in;
|
||
end else begin
|
||
state <= WAIT_ON_ADC;
|
||
timer <= 0;
|
||
adc_arm <= 1;
|
||
end
|
||
end
|
||
WAIT_ON_ADC: if (adc_finished) begin
|
||
adc_arm <= 0;
|
||
arm_mul <= 1;
|
||
state <= WAIT_ON_MUL;
|
||
end
|
||
WAIT_ON_MUL: if (mul_finished) begin
|
||
arm_mul <= 0;
|
||
dac_arm <= 1;
|
||
state <= WAIT_ON_DAC;
|
||
end
|
||
WAIT_ON_DAC: if (dac_finished) begin
|
||
state <= WAIT_ON_ARM;
|
||
dac_arm <= 0;
|
||
end
|
||
end
|
||
|
||
endmodule
|