/* ************************************************************************************************
	Project:			DigiPid
	
	Birth:			2016 Oct
	
	Place:			University of Toronto, Physics, J. Thywissen labs
	
	Design:			Alan Stummer
	
	Description:	The Digital PID ("DigiPid") project is a general purpose closed loop control for
						laser wavelength and power control. There is an ADC and a DAC, both running at up
						to 2.5MSPS. A MCU sets all operating parameters and reads back operating data.
	
	Modus Operandi:	There are two main loops. The MCU loop sets operating parameters and reads
							back data. The runtime loop reads the ADC, runs a PID algorithm and sets the
							DAC. A minor loops measures the VCO and external reference frequency used to
							adjust the main 24MHz clock.
	
	Notes:  All DAC calculations and registers are 16-bit. However, the current DAC only uses the
	        14-MSB but can be upgraded to 16-bit later. DAC=0=-10V, DAC=65535=+10V
*  ***********************************************************************************************/

module UberDigiPid (
	inout  [15:0] McuData,												//MCU data bus
	input   [4:0] McuAddr,												//MCU register address bus
	input         McuRD,													//MCU reads a FPGA registers
	input         McuWR,													//MCU writes a FPGA register
	input         Clk24,													//main 24MHz clock
	input         ClkPll,												//system clock from PLL, probably 100MHz
	input         TtlIn0,												//fixed TTL input (used for Stop)
	input  [15:0] AdcData,												//data from ADC
	input         AdcBusy,												//ADC is busy
	input  [31:0] VcoCounter,											//Measured VCO frequency
	input  [31:0] RefCounter,											//Measured reference frequency (TTL#2)
	input         Clk10,													//PLL generated 10MHz
	input         VcoMonitor,											//VCO ÷4 input
	input  [15:0] HistAdc,												//read from ADC history
	input  [15:0] HistDac,												//read from DAC history
	output [15:0] DacData,												//data to DAC, although it only uses [15:2]
	output        DacClk,												//clock to DAC
	output        nRfOutEn,												//RF output enable, active low
	output        TP1, TP2,												//test points
	output        TtlOut,												//fixed TTL output
	inout         TtlIO1,												//TTL I/O #1
	inout         TtlIO2,												//TTL I/O #2
	output        nAdcCnvst,											//ADC start conversion (active low)
	output        FreqEn, FreqClr,									//frequency counter, clear and enable
	output  [9:0] HistAddrWr, HistAddrRd,							//history read and write addresses
	output        HistWrEn												//history Write enable
);

	/********** SET VERILOG VERSION NUMBER HERE ***************************************************/
	localparam VERSION = 16'd20;										//SET VERILOG VERSION NUMBER HERE
	/**********************************************************************************************/
	
	//GLOBALS
	//N.B.: int is signed 32-bit
	//N.B. Mr Memory: bit is 2-state, reg is 4-state
	bit [15:0] LocalReg [31:0];										//the 32 16-bit registers, R/W by MCU
	bit [15:0] Data4Mcu;													//buffer to hold data to sent to MCU
	bit [15:0] DacMax, DacMin;											//DAC max and min values
	bit [15:0] XferTmp;													//intermediate DAC values during interpolation
	bit [15:0] GainM, GainP, GainI, GainD;							//Gains: Main, Proportional, Integral and Derivative
	int        ErrP,  ErrI, ErrI1, ErrI2, ErrD, LastErrD, Err;//calculation of P, I and D errors, last D error and total error, signed 32-bit
	int        DacTmpRun;												//calculated DAC value in Run mode, might overflow
	int        Adc;														//the latest and greatest ADC value as an int
	int        AdcTarget;												//PID tries to get the ADC to this value, both as unsigned 32-bit ints
	bit [15:0] PidClock;													//PID clock, steps at 100MHz clock rate, cycles at PID rate
	bit  [1:0] ModeOp;													//operating mode: 0=stop, 1=pulse, 2=manual, 3=run
	bit        ModeOut;													//output mode: 0=DC output, 1=RF output
	bit        StatusRead, StatusReadLast;							//flag changed says MCU has read status word, and its previous state
	bit [31:0] PulseCount;												//pulse cycle counter
	bit [31:0] OneSectr;													//1 second timer to measure frequencies, based on 24MHz clock
	bit        HistorySelect;											//select between writing ADC (0) or DAC (1)
	bit        FreezeHistory;											//flag to freeze writing to history while reading them
	bit        FreezeFreqCounters;									//flag to freeze freq counters while reading frequencies

	
	//ASSIGNMENTS
	assign DacClk        = ClkPll;									//always update the DAC
	assign McuData       = (McuRD) ? Data4Mcu : 16'bz;			//MCU data bus tristate control
	assign nRfOutEn      = !(!TtlIn0  &&  ModeOut);				//RF output on only in RF mode and TTL Stop input not active
	assign XferOutClkRd  = ClkPll;									//clock the xfer readback on every runtime clock cycle, whether needed or not
	assign XferOutClkWr  = (McuWR && McuAddr == 5'd20);		//clock in output transfer curve writing on register address 20
	assign XferOutWrEn   = (McuAddr == 5'd20);					//allow output transfer curve writing on address 20
	assign TP1           = FreezeHistory;							//test point #1
	assign TP2           = HistAddrRd[0]; //@@(PidClock < 16'd256) ? 1 : 0;		//test point #2
	assign HistWrEn      = !FreezeHistory;							//don't update histories when reading them
	assign TtlOut        = Clk10;										//10MHz TTL output

	
	//CONSTANTS
	localparam int IaccumMax  = 65000;								//integral gain error accumulator max saturation
	localparam int IaccumMin  = 0;									//integral gain error accumulator min saturation
	
	
	//MODULE INSTANTIATIONS
	DpRam XferOut (														//Output Transfer Curve LUT
		.q      (XferOutRdData),										//note that this LUT is 1024 points...
		.d      (McuData),												//...but DAC is 65K, so interpolated
		.addrRD (XferOutRdAddr),
		.addrWR (XferOutWrAddr),
		.WrEn   (XferOutWrEn),
		.clkWR  (XferOutClkWr),
		.clkRD  (XferOutClkRd)
	);



/****************************** MCU Reads Or Writes FPGA Registers *******************************/
always @(posedge McuWR or posedge McuRD)  begin
	if (McuRD)  begin						//*********************** READ A REGISTER *******************/
		case (McuAddr)
			5'd7:  begin																//Read Status Word and Reset Pointers
				Data4Mcu      <= LocalReg[7];
				StatusRead    <= !StatusRead;										//toggle flag
				XferOutWrAddr <= 10'd0;												//go to start of output transfer curve entry for adding entries
			end
			
			5'd19:  begin																//Read A ADC or DAC History word
				if (HistorySelect)  Data4Mcu <= HistDac;						//(unsigned 16-bit)
				else                Data4Mcu <= HistAdc;						//(unsigned 16-bit)
				HistAddrRd <= HistAddrRd + 10'd1;								//go to next history to read (MCU will read this new addr)
			end

			5'd20:  ;																	//Read An Output Transfer Curve Word
				//The MCU does not read this LUT, it has a copy already

			5'd21:  Data4Mcu <= VERSION;											//Read Verilog version word
				
			5'd22:  Data4Mcu <= Adc;												//Current Raw ADC Data (ADC runs in Run and Test modes only)
				
			5'd23:  Data4Mcu <= DacData;											//Current Raw DAC Data
				
			default:  Data4Mcu <= LocalReg[McuAddr];							//all other regs just send the register
		endcase
	end
		
	
	else  begin								//*********************** WRITE A REGISTER *******************/
		case (McuAddr)
			5'd0:  begin																//Write DAC Maximum Value
				DacMax      <= McuData;
				LocalReg[0] <= McuData;												//save the value so can be read by MCU
			end

			5'd1:  begin																//Write DAC Minimum Value
				DacMin      <= McuData;
				LocalReg[1] <= McuData;												//save the value so can be read by MCU
			end

			//5'd2: ;																	//read only, read VCO freq MS
			//5'd3: ;																	//read only, read VCO freq LS
			//5'd4: ;																	//read only, read reference freq MS
			//5'd5: ;																	//read only, read reference freq LS

			5'd6:  begin																//Write Modes and Flags
				ModeOp             <= McuData[1:0];								//output mode (stop, run, pulse, manual)
				                    //McuData[2] is not used
				ModeOut            <= McuData[3];								//operating mode (direct output or RF)
				FreezeFreqCounters <= McuData[4];								//flag to freeze counter so can read frequency
				                    //McuData[5] is not used
				FreezeHistory      <= McuData[6];								//allow or freeze writing DAC and ADC histories
				HistorySelect      <= McuData[7];								//select to write ADC (0) or DAC (1)
				                    //McuData[15:8] not used
				LocalReg[6]        <= McuData;									//save the value so can be read by MCU
				if (McuData[6])  HistAddrRd <= HistAddrWr;					//if not writing history, sync history read addresses to last written
			end
				
			//5'd7: ;																	//read only, read Status

			5'd8:  begin																//Save DAC Value When Stopped
				LocalReg[8]  <= McuData;											//save the value so can be read by MCU
			end

			//5'd9:  ;																	//future use

			5'd10:  begin																//Save Pulse clock cycles per period, using ClkPll/2^12 (about 24.414KHz)
				LocalReg[10] <= McuData;
			end

			5'd11:  begin																//Save Pulse clock cycles when on (duty cycle), using ClkPll/2^12 (about 24.414KHz)
				LocalReg[11] <= McuData;
			end

			5'd12:  begin																//Save DAC value when manually set
				LocalReg[12] <= McuData;
			end

			5'd13:  begin																//Save Main Gain
				GainM        <= McuData;
				LocalReg[13] <= McuData;
			end

			5'd14:  begin																//Save Divisor for main clock to make PID clock
				LocalReg[14] <= McuData;
			end

			5'd15:  begin																//Save ADC Target For PID
				AdcTarget    <= int'(McuData);
				LocalReg[15] <= McuData;
			end

			5'd16:  begin																//Save Gain, Proportional
				GainP        <= McuData;
				LocalReg[16] <= McuData;
			end

			5'd17:  begin																//Save Gain, Integral
				GainI        <= McuData;
				LocalReg[17] <= McuData;
			end

			5'd18:  begin																//Save Gain, Derivative
				GainD        <= McuData;
				LocalReg[18] <= McuData;
			end

			//5'd19: ;																	//read only, read ADC or DAC history

			5'd20:  begin																//Save Next Output Transfer Curver LUT Entry
				XferOutWrAddr <= XferOutWrAddr + 10'd1;						//point to next, MCU will prevent rollover
			end

			default:  ;																	//NOT A WRITEABLE ADDRESS, DO NOTHING
		endcase
	end
end



/****************************** Runtime Loop ******************************************************/
enum {READ_ADC, GET_ERROR, GAIN_P, GAIN_I1, GAIN_I2, GAIN_I3, GAIN_I4, GAIN_D, DAC1, DAC2, DAC3, DAC4,
      LUT1, LUT2, LUT3, LUT4, LUT5, LUT6, LUT7, LIMIT1, LIMIT2, HISTORY
     } runstates; //Note that 100MHz clock has 40 cycles for 2.5MSPS (fastest, for 1MHz analog BW)

always @(posedge ClkPll)  begin
	if (PidClock == LocalReg[14])  PidClock <= 16'd0;						//generate PID clock (period is 2.5 times bandwidth)
	else                           PidClock <= PidClock + 16'd1;

	case (ModeOp)
		/********** Stop Mode ************/
		2'd0:  begin																	//STOP MODE
			if (!PidClock)  begin													//update at PID rate
				DacData   <= LocalReg[8];											//DAC output is set and limited by MCU
				ErrI      <= 0;														//clear accumulated Intergral gain error
				LastErrD  <= 0;														//clear last derivative error
				DacTmpRun <= int'(DacData);										//starting point when switch to Run mode
				if (!FreezeHistory)  HistAddrWr <= HistAddrWr + 10'd1;	//Save In Histories
			end			
		end


		
		/********** Pulse Mode **********/
		2'd1:  begin																	//PULSE OUTPUT (pulses betweem min and max)
			if (!PidClock)  begin													//update at PID rate
				if (PulseCount)  PulseCount <= PulseCount - 32'd1;			//wait...
				else  			  PulseCount <= {4'd0, LocalReg[10], 12'd0};//reset pulse step rate counter (steps in ClkPll/2^12 = 24.414KHz)
				if (PulseCount < {4'd0, LocalReg[11], 12'd0})				//if before duty cycle...
					DacData <= DacMax;												//...output is high...
				else
					DacData <= DacMin;												//...else low
				ErrI     <= 0;															//clear accumulated Intergral gain error
				LastErrD <= 0;															//clear last derivative error
				if (!FreezeHistory)  HistAddrWr <= HistAddrWr + 10'd1;	//Save In Histories
			end			
		end

		

		/********** Manual Mode *********/
		2'd2:  begin
			if (!PidClock)  begin													//update at PID rate
				DacData   <= LocalReg[12];											//DAC output is set and limited by MCU
				ErrI      <= 0;														//clear accumulated Intergral gain error
				DacTmpRun <= int'(DacData);										//starting point when switch to Run mode
				if (!FreezeHistory)  HistAddrWr <= HistAddrWr + 10'd1;	//Save In Histories
			end			
		end



		/********** Run Mode ************/
		//NB: Although the ADC and DAC are unsigned 16-bit, calculations are done in signed 32-bit then chopped to unsigned 16-bit
		2'd3:  begin																	//RUN, THE PID ALGORITHM, THE RAISON D'ETRE
			case (PidClock)															//PidClock counts up at main clk rate (~100MHz), cycles at PID clk rate (2KHz-2MHz)
				READ_ADC:  begin
					Adc <= int'(AdcData);											//Read the ADC as an int, so that all calculations use the same value
					if (StatusRead != StatusReadLast)  begin					//MCU just read status word so clear flags
						LocalReg[7][0] <= 0;											//DAC overflow
						LocalReg[7][1] <= 0;											//DAC underflow
						LocalReg[7][2] <= 0;											//integral error accumulator overflow
						LocalReg[7][3] <= 0;											//integral error accumulator underflow
					end
					StatusReadLast <= StatusRead;									//save current status read flag
				end

				GET_ERROR:  begin														//Get All Errors (signed 32-bit)
					ErrP  <= AdcTarget - Adc;
					ErrI1 <= AdcTarget - Adc;
					ErrD  <= AdcTarget - Adc;
					nAdcCnvst <= 0;													//start the next ADC conversion (>20nS low)
				end


				//*** Proportional Gain ***
				GAIN_P:  ErrP <= ErrP * int'(GainP);							//(signed 32-bit)


				//*** Integral Gain ***
				GAIN_I1:  ErrI <= ErrI + ErrI1;									//accumulate the error (signed 32-bit)
				
				GAIN_I2:  if (GainI == 0) ErrI <= 0;							//if no I gain, remove accumulated I error
				
				GAIN_I3:  begin  
					if (ErrI > IaccumMax)  begin									//Integral Error Accumulator Positive Overflow
						ErrI           <= IaccumMax;								//cap at max
						LocalReg[7][2] <= 1;											//flag the overflow
					end
					else  if (ErrI < IaccumMin)  begin							//Integral Error Accumulator Negative Overflow
						ErrI           <= -IaccumMin;								//cap at min
						LocalReg[7][3] <= 1;											//flag the underflow
					end
				end

				GAIN_I4:  ErrI2 <= ErrI * int'(GainI);							//Scale Error
				

				//*** Derivative Gain ***
				GAIN_D:  begin															//Derivative Gain Error (signed 32-bit)
					ErrD     <= (ErrD - LastErrD) * int'(GainD);
					LastErrD <= ErrD;
				end


				//*** Implement Errors Into DAC And Limit to ±10V ***
				DAC1:  Err <= (ErrP + ErrI2 + ErrD) * int'(GainM);			//Sum All Errors (signed 32-bit)
				
				DAC2:  DacTmpRun <= DacTmpRun + (Err / 128);					//Scale And Add In All Errors (signed 32-bit)
				
				DAC3:  begin															//Handle Positive Overflow
					if (DacTmpRun > int'(65535))  begin
						DacTmpRun <= int'(65535);									//limit to +10V
						LocalReg[7][0] <= 1;											//set overflow flag
					end
					nAdcCnvst <= 1;													//remove ADC start
				end
				
				DAC4:  begin															//Handle Negative Overflow
					if (DacTmpRun < int'(0))  begin
						DacTmpRun <= int'(0);										//limit to -10V
						LocalReg[7][1] <= 1;											//set underflow flag
					end
				end
				
			
				
				//*** Apply Output Transfer Curve *** (Interpolate LUT's 2^10 entries, 64 DAC values per LUT value)
/*				LUT1:  XferOutRdAddr <= DacTmpRun[15:6] + 1;					//Select Next Higher Table Entry
				
				LUT2:  if (XferOutRdAddr > 1023)  XferOutRdAddr = 1023;	//Limit Range to 1024 Entries

				LUT3:  XferTmp <= XferOutRdData;									//Get Next Higher Value From Transfer Curve LUT

				LUT4:  XferOutRdAddr <= XferOutRdAddr - 1;					//Select Next Lower Table Entry

				LUT5:  XferTmp <= XferTmp - XferOutRdData;					//Get Difference Between Higher and Lower Values
				
				LUT6:  XferTmp <= XferTmp * (DacTmpRun & 32'h0000003F);	//Multiply By Modulus 64 of DAC (to get how far between LUT points)
				
				LUT7:  DacTmpRun <= DacTmpRun + XferTmp;						//Add Interpolated part to Calculated DAC
				
*/

				//*** Limit to min and max values *** (remember DAC 0=-10V, 65535=+10V) ******************
				LIMIT1:  begin
					if (DacTmpRun > int'(DacMax))  begin						//DAC Overflow (too positive)
						DacTmpRun <= int'(DacMax);									//cap at max value
						LocalReg[7][0]  <= 1;										//set DAC overflow flag
					end
					else  if (DacTmpRun < int'(DacMin))  begin				//DAC Underflow (too negative)
						DacTmpRun <= int'(DacMin);									//cap at min value
						LocalReg[7][1]  <= 1;										//set DAC underflow flag
					end
				end

				LIMIT2:  DacData <= DacTmpRun[15:0];							//Finally, Output The DAC (unsigned 16-bit)


				
				//*** Save In Histories ***
				HISTORY:  begin														//Save ADC And DAC Histories
					if (!FreezeHistory)  HistAddrWr <= HistAddrWr + 10'd1;//Save In Histories
				end
				
			endcase
		end
	endcase		
end


/****************** VCO And External Reference Frequency Measurement *****************************/
always @(posedge Clk24)  begin
	if (OneSectr == 32'd24000005)  OneSectr <= 32'd0;						//restart
	else  OneSectr <= OneSectr + 32'd1;											//count for 1 sec

	case (OneSectr)																	//COUNT FOR 1 SECOND AND SAVE RESULTS
		32'd0:  FreqEn <= 0;															//Stop Counting
			
		32'd1:  begin																	//Load Frequencies Into Local Registers
			if (!FreezeFreqCounters)  begin
				LocalReg[2] <= VcoCounter[31:16];								//VCO, MS (has ÷4 prescaler)
				LocalReg[3] <= VcoCounter[15: 0];								//VCO, LS
				LocalReg[4] <= RefCounter[31:16];								//Ref input, MS (no prescaler)
				LocalReg[5] <= RefCounter[15: 0];								//Ref input, LS
			end
		end

		32'd2:  FreqClr <= 1;														//Clear Counter	
	
		32'd3:  FreqClr <= 0;														//Don't clear Counter
		
		32'd4:  FreqEn  <= 1;														//Allow Counting
		
		default: ;
	endcase
end

endmodule



/****************** Dual Port RAM Module, 16-bit x 1024 words ************************************/
module DpRam  (
   output[15:0] q,
   input [15:0] d,
   input [9:0] addrRD,
   input [9:0] addrWR,
   input WrEn, clkWR, clkRD
	);
 
   reg [15:0] mem [1023:0];
 
   always @(posedge clkWR)  if (WrEn) mem[addrWR] <= d;					//WRITE
 
   always @(posedge clkRD)  q <= mem[addrRD];								//READ
        
endmodule