OV7670 Camera Shield VerII FPGA Servo Controller

An FPGA implemented smooth servo controller for Arduino Nano 33 OV7670 Camera Shield.

Mar 3, 2021

•

3843 views

•

0 respects

Components and supplies

Nano 33 BLE Sense

OV7670 Camera

Mouser Project

15pin pin header

JST 2.0 2pin cable and connecter

Tools and machines

Soldering iron (generic)

Reflo Air

Project description

Code

FPGA verilog

verilog

1/* ------------------------------------------------------------------
2    FPGA code
3   MEDAMA PROJECT
4   PORT/FPGApin  ASSIGNMENT
5                     +-fpga-----+
6                      |MACHXO2   8--->RGB(D7)
7           (A7)IR--->11         5--->BUZ(A6)
8                      |         20--->SV0
9                     |         21--->SV1
10                      |         14--->SV2(A3) can use for monitor
11                     |          13--->SV3(A2) can use for monitor
12    +-cam-------+    |          |     +-mpu----------+
13    |OV767X     |    |   XCLK2 23<---D9  XCLK nano33|
14     |           |    |      RX 25<---D12 TX   3.3V  |
15    |        XCLK<---4   XCLK TX 26--->D11 RX         |
16    |           |    |          |    |              |
17     |        PCLK--->9 PCLK    10--->A0 DPCLK       |
18    |       VSYNC--->16   VSYNC  17--->D8 DVSYNC  A4/A5
19    I2C-BUS-HREF+    +---12-----+    +-A1--D0-7--I2C+
20       |   |   +-----------+-HREF-------+     |    |
21      |   +-----------------D0-D7-------//8--+     |
22      +---------------------I2C-SDA[27]SCL[28]A5--+
23   -----------------------------------------------------------------*/
24module   TinyFPGA_A2 (       				
25				input 	IR,			// pin11 ->IR{A7)
26				output	TP14,		//   pin14 ->SV2(A3)
27				output	TP13,		// pin13 ->SV3(A2)
28		
29				output	SV0,		//   pin20 ->
30				output	SV1,		// pin21 ->
31				output	BUZZ,		// pin5   ->BUZZ(A6)
32				output	RGB,		// pin8  ->RGB(D7)
33
34                input   	XCLK2,   	// pin23 <-D9 16MHz			
35				output	XCLK,		// pin4  ->cam   15MHz
36				input	PCLK,		// pin9  <-cam
37				output	DPCLK,		//   pin10 ->A0 =PCLK
38				input	VSYNC,		// pin16 <-cam
39				output	DVSYNC,		//   pin17 ->D8 =VSYNC
40				input	HREF,		// pin12(A1) <-cam
41			
42				input   	RX,       	// pin25 <-D12(TX@HOST)
43				output	TX			// pin26 ->D11(RX@HOST)   TXTP26
44					);
45 /***********************************************************************
46   * CAMERA logic                                                         *
47 ***********************************************************************/
48   //reg [2:0] HAFC;  always @(posedge clk) HAFC <= HAFC + 1;
49  //assign XCLK   = HAFC[0];			//       (15MHz)     [4] -> XCLK 
50  assign DPCLK = PCLK;				//   PCLK ------> [9] [10] -> DPCLK(A0) 
51  assign DVSYNC = VSYNC;			// VSYNC ----->   [16][17] -> DVSYNC(D8)
52									// HREF -(A1)-> [12]
53									//   XCLK(library)---> D9
54  //assign XCLK = XCLK2;			// 16MHz
55  assign XCLK   = CLK;				// 30MHz
56  //assign TP14 = HREF;	// PIN14  (A3)
57  //assign   TP13 = VSYNC;	// PIN13  (A2)
58 /***********************************************************************
59   * SYSTEM CLOCK  29.56(33.8)/26.60(37.6)/24.18(41.3)/22.17(45.1)/20.46(48.9)
60   ***********************************************************************/	  
61   wire CLK;  // base clock 29.56MHz 33.25ns
62  OSCH #(.NOM_FREQ("29.56")) internal_oscillator_inst   (.STDBY(1'b0), .OSC(CLK)); // 
63  assign clk = CLK;  
64  /***********************************************************************
65   * IR receiver                  32bit from MSB to LSB
66 --|_9ms_start_|-4ms-|_600us_|-("0")600us-|_600us_|-("1")1600us-|_600us_stop_|--   
67 ***********************************************************************/
68   wire IRn; reg IRnn;			// IR pin/glidge canceller
69  assign IR = IRn;				//   pin11
70
71  reg [7:0] IRcntL, IRcntH;	// input signal glidge canceller
72  always   @(posedge clk) begin 	// remove glidge
73	if(IRn == 0) begin IRcntL <= IRcntL   + 1; IRcntH <= 0;	end
74	if(IRn == 1) begin	IRcntH <= IRcntH + 1; IRcntL <=   0;	end
75	if(IRcntL > 20) IRnn <= 0;    if(IRcntH > 20) IRnn <= 1;
76  end
77   
78  reg [31:0] IRreg; 			// 32bit receive data buffer
79  reg [7:0] rep;				//   decoded 8bit char data (to uart)
80  reg [5:0] irseq; 			// ir receive sequence   
81  
82  reg [19:0] ctirL, ctirH; 	// l/h clock(33.8ns) counter 
83  reg ena,   pena;				// enable gate flags
84  reg Ureq;						// uart request
85   always @(posedge clk) begin	
86	// enable signal from Startbit && H
87	if(ctirH   > 20'h30000) begin pena <= 0; ena <= 1'b0; irseq <= 0; 	end // idle 6.4ms
88	if(ctirL   > 20'h30000) begin pena <= 1;        					end // prenable
89	if(pena == 1   && IRnn == 1) begin ena <= 1'b1;         			end	// enable
90	if(Ureq == 1   && TxD_data == 0) Ureq <= 1'b0; // reset uart transfer request @TxD started
91	//   receive 32 bit data
92	if(ena == 1) 	begin
93	if(IRnn == 0) 	begin	// "L"   ---- 
94		if(ctirL == 0) begin //
95			if(irseq > 0) begin
96			if(ctirH   > 18'h4557) 	begin IRreg[32-irseq[5:0]] = 1'b1; end // > 600us 
97			else                 	begin   IRreg[32-irseq[5:0]] = 1'b0; end // < 600us		
98			if(irseq == 32) begin 		//		
99				Ureq   <= 1'b1; 	   		// Uart request pulse on
100				rep <= IRreg[7:0]; 	// LSB   8bit
101  				//case(IRreg[15:0]) // decode data 32bit >> char 8bit
102				//	16'hA25D:   rep <= 8'h31; // "1(0x31)" 
103				//	16'h629D: rep <= 8'h32; // "2(0x32)"   
104				//	16'hE21D: rep <= 8'h33; // "3(0x33)" 
105				//	16'h22DD:   rep <= 8'h34; // "4(0x34)" 
106				//	16'h02FD: rep <= 8'h35; // "5(0x35)"   
107				//	16'hC23D: rep <= 8'h36; // "6(0x36)" 
108				//	16'hE01F:   rep <= 8'h37; // "7(0x37)" 
109				//	16'hA857: rep <= 8'h38; // "8(0x38)"
110				//	16'h906F:   rep <= 8'h39; // "9(0x39)" 
111				//	16'h9867: rep <= 8'h30; // "0(0x30)"   
112				//	16'h6897: rep <= 8'h2A; // "*(0x2A)" 
113				//	16'hB04F:   rep <= 8'h23; // "#(0x23)" 
114				//	16'h18E7: rep <= 8'h55; // "U(0x55)"   
115				//	16'h4AB5: rep <= 8'h44; // "D(0x44)" 
116				//	16'h10EF:   rep <= 8'h4C; // "L(0x4C)" 
117				//	16'h5AA5: rep <= 8'h52; // "R(0x52)"   
118				//	16'h38C7: rep <= 8'h4B; // "K(0x4B)" *"C(0x43)":SVO COMP
119				//endcase			
120			end
121			end
122		end
123	end   //"H" ----	 next step
124	else if(ctirH == 0) irseq <= irseq + 1; 	
125	end
126	//   L/H pulse width counters
127	if(IRnn == 0) 	begin				// "L" ---- 
128		if(ctirL   != 0) ctirH <= 0; 	// first entry after H >> L(counter == 0)
129		if(ctirL <   20'h45000) ctirL <= ctirL + 1; 	// "L" timer count up	9ms
130	end else  		begin				//   "H" ----	
131		if(ctirH != 0) ctirL <= 0; 	// first entry after L >> H(counter   == 0)
132		if(ctirH < 20'h45000) ctirH <= ctirH + 1; 	// "H" timer count up
133	end
134   end
135 /***********************************************************************
136   * UART Transfer    output TX[25]--D12 RX@HOST  input RX[23]--D9 TX@HOST         
137   [IR command to UART command transfer]                
138    -->Receive IR data   v     reset by TxL
139	                __X-[Ureq]-X_ v^
140					   Start Uart   Tx transfer
141 ***********************************************************************/	
142   //reg [10:0] BaudDivCnt;				// [10:0] 11bit 14400 tick 69.6(67.8us)us
143   reg [7:0] BaudDivCnt;				// [7:0] 8bit 115200 tick 8.7(8.5us)us
144  wire   BaudTick = (BaudDivCnt == 15); 	// 
145
146  reg [7:0] Ustate;			// state machine
147   reg TxD_start, TxD_data;		// data line
148  reg [7:0] Udata;				// Transfer   data rep[7:0];
149  assign TX = TxD_data;		// pin26 to D11
150
151  always @(posedge   clk) begin
152  BaudDivCnt <= BaudDivCnt + 1;
153  if(Ustate == 0) TxD_data  <=   0'b1;  // preset
154  //- receive transfer request -------------------------
155   if(Ureq && TxD_start == 0) 	begin //receive req/preset
156	  TxD_start <= 1'b1;   Ustate <= 8'h00; Udata <= rep; 	// load transfer data (IR data/Trans comp)
157   end
158  if(SCreq && TxD_start == 0) 	begin //servo comp req/preset
159	  TxD_start   <= 1'b1; Ustate <= 8'h00; Udata <= 8'h43; // "C" servo comp report
160  end
161   //- state machine-------------------------------------
162  if(TxD_start && BaudTick)   Ustate <= Ustate + 1;
163  case(Ustate)
164	 8'h03: TxD_data <= 0'b0;		// Start   bit
165	 8'h04: TxD_data <= Udata[0];	// out bit0
166	 8'h05: TxD_data <= Udata[1];	//   out bit1
167	 8'h06: TxD_data <= Udata[2];	// out bit2
168	 8'h07: TxD_data <=   Udata[3];	// out bit3
169	 8'h08: TxD_data <= Udata[4];	// out bit4
170	 8'h09:   TxD_data <= Udata[5];	// out bit5
171	 8'h0A: TxD_data <= Udata[6];	// out bit6
172	   8'h0B: TxD_data <= Udata[7];	// out bit7
173	 8'h0C: TxD_data  <= 0'b1;		//   out stopbit
174	 8'h0D: TxD_data  <= 0'b1;		// out stopbit
175	 8'h0E: TxD_start   <= 1'b0; 	// complete and close
176  endcase
177  end
178 /***********************************************************************
179   * UART Receive     output TX[25]--D12 RX@HOST  input RX[23]--D9 TX@HOST         
180	ff   no action/00 reset counter
181 ***********************************************************************/
182   //reg [9:0] BaudDivCntR; 					// [9:0] 10bit 14400 double tick
183  reg [6:0]   BaudDivCntR;					// [6:0] 7bit 115200 tick 8.7us/2
184  wire BaudTickR = (BaudDivCntR   == 15); 	//
185
186  reg RxD_start;
187  wire RxD_data = RX; 			// loop back   test
188  reg [5:0] Ubp;				// buffer pointer 8x5byte 0-40
189  reg [6:0] UstateR;			//   state machine
190  reg [39:0] URdtR;			// 5bytex8 data
191  reg monit;					//   debug flag
192  reg [3:0] CMD;				// command reg trasfer command to servo control
193   
194  always @(posedge clk) begin
195  BaudDivCntR <= BaudDivCntR + 1;
196  if(RxD_data   == 0 && RxD_start == 0) begin 	//receive request/preset
197	   BaudDivCntR <=   14; // Sync with received RxD_data start
198	   RxD_start <= 1'b1;  UstateR <=   8'h00; 	// need reset source
199	   //monit <= 0;
200  end 	
201			
202  if(RxD_start   && BaudTickR) UstateR <= UstateR + 1;
203  case(UstateR)
204	 8'd3:  if(BaudTickR)   begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit0
205	 8'd5:  if(BaudTickR)   begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit1
206	 8'd7:  if(BaudTickR)   begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit2	 
207	 8'd9:  if(BaudTickR)   begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit3
208	 8'd11: if(BaudTickR)   begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit4
209	 8'd13: if(BaudTickR)   begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit5	 	 
210	 8'd15: if(BaudTickR)   begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit6
211	 8'd17: if(BaudTickR)   begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit7
212	 8'd19: if(BaudTickR)   begin	// stop bit, store to each registers
213		if(Ubp == 8'd40) begin
214			Ubp   <= 0; 				// Reset counter
215			case(URdtR[7:0]) 		// byte0
216				8'h01:   begin		// servo position command 0xff:do nothing
217					CMD <= 8'h01;	//   set command received
218					if(URdtR[15:8] != 8'hff)	chNSVO[0] <= URdtR[15:8];	//   SV0 target angle	 	
219					if(URdtR[23:16] != 8'hff)	chNSVO[1] <= URdtR[23:16];	//   SV1 target angle
220					if(URdtR[31:24] != 8'hff)	chNSVO[2] <= URdtR[31:24];	//   SV2 target angle	 	
221					if(URdtR[39:32] != 8'hff)	chNSVO[3] <= URdtR[39:32];	//   SV3 target angle
222					   end
223				8'h02: begin		// servo speed command
224					CMD   <= 8'h02;  // set command received
225					if(URdtR[15:8] != 8'hff)  SVsped   <= URdtR[15:8];	 // wait counter for delay
226					if(URdtR[23:16] != 8'hff)   SVstop <= URdtR[23:16]; // 0000xxxx target <= current
227					else SVstop <=   0;									 // pause flag reset
228					if(URdtR[31:24] != 8'hff)   SVswng <= URdtR[31:24]; // mode/depth(6)	
229					   end
230				8'h03:   begin 		// RGB led command
231					RGBR[23:16] <= URdtR[23:16]-1;	// G
232					RGBR[15:8]	<=   URdtR[15:8]-1; 		// R
233					RGBR[7:0] 	<= URdtR[31:24]-1; 	// B
234					    end
235				8'h04: begin 		// BUZ command
236					BuzNote <= URdtR[15:8];		//   0x96,97,98,99,9A,9B noTon 0x0f
237					BuzLgth <= URdtR[23:16]; 		// Option   bit7:enable svo/led pulse
238					   end
239			endcase
240		end
241		else   begin // reset receive data buffer pointer
242			if(		(Ubp == 8'd8) &&(URdtR[7:0]   	== 8'h00)	// closing @1st byte
243				|| 	(Ubp == 8'd16)&&(URdtR[15:8]	==   8'h00)	// closing @2nd byte
244				|| 	(Ubp == 8'd24)&&(URdtR[23:16]	== 8'h00)   	// closing @3rd byte
245				|| 	(Ubp == 8'd32)&&(URdtR[31:24]	== 8'h00)   	// closing @4th byte
246			) begin Ubp <= 0;  end // Reset counter
247		end
248	   end
249	 8'd21: begin RxD_start <= 1'b0; CMD<= 0; end	// command received pulse   4us
250  endcase
251  end
252  //--monitor-----------------------------------------
253   //assign TP14 = svocpm[4];	// PIN14  (A3)
254  //assign TP14 = monit;	// PIN14   (A3)
255  //assign TP13 = RxD_data;		// PIN13  (A2)
256  //--------------------------------------------------
257   /***********************************************************************
258 * Connection   REGISTER SET                      // xff:no load/0:no if  *
259 ***********************************************************************/   
260  reg [7:0] chNSVO[3:0];						// SERVO TARGET 0x01/byte1/byte2/byte3/byte4(1-0xfe)
261   reg [7:0] SVsped, SVstop, SVswng; 	    	// SERVO SPEED 0x02/00010000(),wait/0001111(stp)
262   reg [23:0] RGBR;								// RGB LED 0x03/red/green/blue()0x01-0xfe(-1)
263   reg [7:0] BuzNote, BuzLgth;		 			// BUZ NOTE 0x04/0x96,97,98,99,9A,9B noTon   0x0f
264 /***********************************************************************
265   * SERVO controller     0.5ms---1.5ms---2.5ms                           *
266      ____________   _________                                        ______
267 ____|set         ^         |reset__________reset____//____reset___|set   ^ 
268 con=0xAEFE  con=0xB4A2->0  con[11:2]==act[9:0] 0x0000-0x2d0(0-2ms) con=0xAEFE
269	output	SV0,		//   pin20 ->
270	output	SV1,		// pin21 ->
271   	output	TP14,		// pin14 ->SV2(A3)
272	output	TP13,		//   pin13 ->SV3(A2)
273	BuzLgth[7] ebable pulse
274	BuzLgth[6] ebable On track (stop   servo pulse/issue servo comp)
275 ***********************************************************************/
276   reg [3:0] svOUT;    	   						 	// output signalx4 lines
277    assign   SV0  = svOUT[0];		// pin20
278	assign SV1  = svOUT[1];		// pin21					
279     assign TP14 = svOUT[2]; 		// pin14 SV2
280    assign TP13 = svOUT[3]; 		//   pin13 SV3
281  parameter CYCL = 20'h80000;						// 16ms Servo cycle
282  parameter   PPAL = 16'h3f00;						// 0.5ms Pre Pulse    
283  reg [19:0] svREG; 	     						//   Servo base counter
284  
285  reg [7:0] svTGT[3:0];							// 8bit target   register
286  reg [9:0] svCRT[3:0];							// 10bit current count up
287  reg   [9:0] svocpm; 								// set speed by uart
288  reg onTrk;
289  
290  reg   [1:0] rot;								    // servo ch rotation
291  always @(posedge clk) begin
292	if(rot   == 0) rot <= 1; if(rot == 1) rot <= 2;
293	if(rot == 2) rot <= 3; if(rot == 3)   rot <= 0;
294  end
295	
296  always @(posedge clk) begin
297	if(svREG > 17'h15000   && svREG < 19'h70000) begin // avoid servo positive pulse
298	if(	{svTGT[0], 2'b00}   == svCRT[0] && {svTGT[1], 2'b00} == svCRT[1] &&
299		{svTGT[2], 2'b00} == svCRT[2]   && {svTGT[3], 2'b00} == svCRT[3] && BuzLgth[6] == 1
300	) onTrk <= 1; else onTrk   <= 0;
301	end
302  end
303	
304  always @(posedge clk) begin svREG <= svREG +   1; // timer count up
305	if(BuzLgth[7] == 1) begin 
306      if(onTrk == 0) begin
307	   //-20bit-----4bit----10bit------6bit]------------------------
308	  if(svREG   < {4'h0, svCRT[rot], 6'h0}) svOUT[rot] <= 1'b1; 			
309	  else svOUT[rot] <=   1'b0; 						// PULSE GENERATE0	
310	  if(svREG > CYCL - PPAL + 3000) svOUT   <= 4'hf;	// BASE 0.5ms PULSE				
311	  if(svREG > CYCL) svREG <= 0; 					//   END CYCLE
312	  end else svOUT <= 0;
313	end
314  end
315 /***********************************************************************
316   * us Event Control counter 29.56MHz(33.8ns)                            *
317 ***********************************************************************/
318   reg [14:0] cntU; reg uTick; parameter uDiv = 17750/3; 	// 29.56MHz(33.8ns) >>   2kHz(600us)  
319  always @(posedge clk) uTick <= (cntU == uDiv-2);	   	// counter   reset pulse    
320  always @(posedge clk) if(uTick) cntU <= 0; else cntU <= cntU   + 1; // |19.2|9.6|4.8|2.4|1.2|0.6ms|
321 //---------------------------------------------------------------------
322   reg [9:0] svo[3:0]; 						// wait control
323  reg [9:0] svocp[3:0];						//   compare data delay
324
325  always @(posedge clk) begin // INCREASE/DECREASE position   svCRT[rot]
326	if(uTick) begin  // delay timer COUNT UP/DOWN
327		svo[0] <= svo[0]   + 1'b1; svo[1] <= svo[1] + 1'b1;
328		svo[2] <= svo[2] + 1'b1; svo[3] <= svo[3]   + 1'b1;
329    end	
330	if(svo[rot] > svocp[rot]) begin svo[rot] <= 10'h0;  		//   wait time
331		if({svTGT[rot], 2'b00} > svCRT[rot]) svCRT[rot] <= svCRT[rot] +   1'b1; // increase
332		if({svTGT[rot], 2'b00} < svCRT[rot]) svCRT[rot] <= svCRT[rot]   - 1'b1; // decrease			 
333	end 
334  end
335  
336  always @(posedge clk) begin   // SPEED UP by distance CONTROL  0---30(120)--60{240}--180(720) svocp[rot]
337	if(svocpm[7]   == 0) begin // bit7:disable accel bit6-0:waiting value
338	if({svTGT[rot], 2'b00}   > svCRT[rot]) begin 
339		if({svTGT[rot], 2'b00} - svCRT[rot] > 240) 		svocp[rot]   <= {3'b00, svocpm[7:3]};
340		else if({svTGT[rot], 2'b00} - svCRT[rot] < 100)   	svocp[rot] <= svocpm;
341		else 												svocp[rot] <= {2'b00,   svocpm[7:2]}; 
342	end // increase
343	else if({svTGT[rot], 2'b00} < svCRT[rot])   begin 
344		if(svCRT[rot] - {svTGT[rot], 2'b00} > 240) 		svocp[rot] <= {3'b00,   svocpm[7:3]};
345		else if(svCRT[rot] - {svTGT[rot], 2'b00} < 100)	svocp[rot]   <= svocpm;
346		else 												svocp[rot] <= {2'b00, svocpm[7:2]};   
347	end // decrease 
348	end else svocp[rot] <= svocpm[6:0]; // flat control
349   end
350/***********************************************************************
351   * SERVO COMPLETE uart req                                             *
352 ***********************************************************************/   
353  reg SVCOMP;		// servo complete
354  reg SCreq;		// servo comp report   request pulse
355  reg [2:0] SMS; 	// state
356  always @(posedge clk) begin SMS   <= SMS + 1;  
357	if(swan[3:0] == 0) begin // SVswng[3:0] == 0 enable comp report
358   	case(SMS)
359		3'h01: begin
360			if(onTrk) begin SVCOMP <= 1; if(SVCOMP   == 0) SCreq <= 1; end	// leading edge
361			else SVCOMP <= 0; 
362		end
363		3'h06:   if(SCreq == 1) SCreq <= 0;
364	endcase
365	end
366  end
367 /***********************************************************************
368   * ms Event Control LOAD COMMAND/BREATH move                            *
369  svTGT[3:0],   svocpm	 // target/speed/stop bit INTERNAL
370  chNSVO[3:0]			 // SERVO TARGET   0x01/byte1/byte2/byte3/byte4(1-0xfe)
371  SVsped, SVstop, SVswng // SERVO SPEED   0x02/00010000(),wait/0001111(stp)
372  [UART command to Servo execution]  
373	-->Receive   UART command 5bytes
374	                       X-[CMD]-Xv
375                     load   to [catchCMD]v              ^ reset catch CMD 
376					         load parameter   from command @mTicK
377 ***********************************************************************/
378   reg [14:0] cntM; reg mTick; parameter mDiv = 10000; 	// 400us 28000
379  always   @(posedge clk) mTick <= (cntM == mDiv-2);		// counter reset pulse  
380  always   @(posedge clk) if(mTick) cntM <= 0; else cntM <= cntM + 1'b1;  //
381 //---------------------------------------------------------------------
382   reg [11:0] led;  	// state machine
383  reg [3:0]catchCMD;	// copy of uart command   reg CMD
384  reg [7:0] swan;		// swing parameters polarity4bit + depth 4bit
385   always @(posedge clk) begin
386	  if(mTick && RxD_start == 0) begin led <= led   + 1'b1; 
387	    case(led) // 1sec interval counter sequence control
388		  default:   begin // load uart command every 400us
389			      // access svTGT, sync with   CMD/catchCMD ontime after receiving uart command
390				  // need 1ms interval   after command 0x01/0x02
391				  if(catchCMD == 2'h01) begin 	// angle01/2/3
392					   svTGT[0] <=  chNSVO[0];	// ch0 load target angle from uart
393					 svTGT[1]   <= ~chNSVO[1];	// ch1
394					 svTGT[2] <=  chNSVO[2];	// ch2
395					   svTGT[3] <= ~chNSVO[3];	// ch3
396					 catchCMD <= 0;  			// reset catched   command (ack)
397					 monit <= 0;
398				  end
399				  if(catchCMD   == 2'h02) begin 	// speed/stop/swing
400					 if(SVstop[0] == 1) svTGT[0][7:0]   <= svCRT[0][9:2]; 	// ch0
401					 if(SVstop[1] == 1) svTGT[1][7:0] <= svCRT[1][9:2];   	// ch1
402					 if(SVstop[2] == 1) svTGT[2][7:0] <= svCRT[2][9:2]; 	// ch2
403					   if(SVstop[3] == 1) svTGT[3][7:0] <= svCRT[3][9:2]; 	// ch3
404					 swan <=   SVswng;			//
405					 //svocpm <= 8'b00010000; 	// set data from uart
406					   svocpm <= SVsped;			// load speed reg from uart
407				     catchCMD <= 0;   			// reset catched command (ack)
408					 monit <= 1;
409				  end
410			     end
411		  {4'b0100, 8'h00}: begin 
412			    if(swan[3:0] != 0) begin
413				   if(swan[4] == 0 && svTGT[0] > 32) svTGT[0] = svTGT[0] - {swan[3:0], 1'b0}; //front
414				   else if(svTGT[0]< 223) svTGT[0] = svTGT[0] + {swan[3:0], 1'b0};
415				  if(swan[5]   == 0 && svTGT[1] > 32) svTGT[1] = svTGT[1] - {swan[3:0], 1'b0}; 
416				  else   if(svTGT[1]< 223) svTGT[1] = svTGT[1] + {SVswng[3:0], 1'b0};
417			//	  if(swan[6]   == 0 && svTGT[2] > 32) svTGT[2] = svTGT[2] - {swan[3:0], 1'b0}; // rear
418			//	   else if(svTGT[2]< 223) svTGT[2] = svTGT[2] + {SVswng[3:0], 1'b0}; 
419			//	   if(swan[7] == 0 && svTGT[3] > 32) svTGT[3] = svTGT[3] - {swan[3:0], 1'b0}; 
420			//	   else if(svTGT[3]< 223) svTGT[3] = svTGT[3] + {swan[3:0], 1'b0}; 
421				end
422				end   								//
423		  {4'b1100, 8'h00}: begin 
424			    if(swan[3:0] !=   0) begin
425				  if(swan[4] != 0 && svTGT[0] > 32) svTGT[0] = svTGT[0] - {swan[3:0],   1'b0}; // front
426				  else if(svTGT[0]< 223) svTGT[0] = svTGT[0] + {swan[3:0],   1'b0};
427				  if(swan[5] != 0 && svTGT[1] > 32) svTGT[1] = svTGT[1] - {swan[3:0],   1'b0}; 
428				  else if(svTGT[1]< 223) svTGT[1] = svTGT[1] + {swan[3:0], 1'b0};
429			//	   if(swan[6] != 0 && svTGT[2] > 32) svTGT[2] = svTGT[2] - {swan[3:0], 1'b0}; //   rear
430			//	  else if(svTGT[2]< 223) svTGT[2] = svTGT[2] + {swan[3:0], 1'b0};
431			//	   if(swan[7] != 0 && svTGT[3] > 32) svTGT[3] = svTGT[3] - {swan[3:0], 1'b0}; 
432			//	   else if(svTGT[3]< 223) svTGT[3] = svTGT[3] + {swan[3:0], 1'b0};
433				end
434				end   
435	    endcase
436	  end else begin if(CMD != 0) begin catchCMD <= CMD; end   // catch 4us 0x01/0x02 command pulse
437	  end // catch uart command
438  end
439   /***********************************************************************
440 * RGB   LED controller  G-R-B 24bit [11:8](RGB+5byte blank) [7:5](byte cntr) [4:0](0(10/22)
441   _|-2-500ns-|__650--950ns_____|"0" _|--550--650ns-|__450--750ns_|"1" -|_//_>50us_|-   "reset"  
442  BuzLgth[7] ebable pulse 
443 ***********************************************************************/
444   reg rgbOUT;    	    // output signal
445  assign RGB = rgbOUT;	// [8] D7
446   reg [23:0] RGBreg;	// 3byte RGB color set through uart
447  reg [21:0] ctRGB;	//   rgb clock(33.25ns) 21-8(byte pointer)/9-5(24bit pointer)
448  reg [4:0] dptn; 		//   data pattern "0/1" compare reg
449  always @(posedge clk) begin ctRGB <= ctRGB   + 1;
450	  RGBreg <= RGBR;	// RGBR[23:0]: LED 0x03/red/green/blue()0x01-0xfe(-1)
451	   if(BuzLgth[7] == 1) begin
452	  if(ctRGB[21:8] == 0 || ctRGB[21:8] == 1 || ctRGB[21:8]   == 2) begin // idle @24bit~
453		  if(RGBreg[23-ctRGB[9:5]] == 0)	dptn <= 5'b01010;   	// "0" rgbout = 1'b1; 360/800
454		  else 								dptn <= 5'b10001;   	// "1" rgbout = 1'b1; 580/600
455		  if(ctRGB[4:0] < dptn[4:0]) rgbOUT <=   1; else rgbOUT <= 0;
456	  end else rgbOUT = 1'b0;     							// "0" reset   mode
457	 end
458  end
459 /***********************************************************************
460   * BUZZ controll BuzNote[7:0], BuzLgth[7:0]; // BUZ NOTE 0x04/0x96,97,98,99,9A,9B   noTon 0x0f
461 ***********************************************************************/
462   reg speaker;				// sound pulse
463  assign BUZZ = speaker;	// [5] A6
464   reg [27:0] tone;
465  reg [7:0] ntype; 		// 0x96,97,98,99,9A,9B/0x0f dis
466   always @(posedge clk) begin ntype <= BuzNote; end
467  always @(posedge clk) begin   tone <= tone+1; end
468  wire [6:0] fastsweep = (tone[22] ? tone[21:15] : ~tone[21:15]);
469   wire [6:0] slowsweep = (tone[25] ? tone[24:18] : ~tone[24:18]);
470  wire [14:0]   clkdivider = {2'b01, (tone[ntype[6:0]] ? slowsweep : fastsweep), 6'b000000}; 
471
472   reg [14:0] counter;
473  always @(posedge clk) if(counter==0) counter <= clkdivider;   else counter <= counter-1;
474  always @(posedge clk) if(counter==0 && ntype[7])   speaker <= ~speaker;
475	  
476endmodule
477

/* ------------------------------------------------------------------
    FPGA code
   MEDAMA PROJECT
   PORT/FPGApin  ASSIGNMENT
                     +-fpga-----+
                      |MACHXO2   8--->RGB(D7)
           (A7)IR--->11         5--->BUZ(A6)
                      |         20--->SV0
                     |         21--->SV1
                      |         14--->SV2(A3) can use for monitor
                     |          13--->SV3(A2) can use for monitor
    +-cam-------+    |          |     +-mpu----------+
    |OV767X     |    |   XCLK2 23<---D9  XCLK nano33|
     |           |    |      RX 25<---D12 TX   3.3V  |
    |        XCLK<---4   XCLK TX 26--->D11 RX         |
    |           |    |          |    |              |
     |        PCLK--->9 PCLK    10--->A0 DPCLK       |
    |       VSYNC--->16   VSYNC  17--->D8 DVSYNC  A4/A5
    I2C-BUS-HREF+    +---12-----+    +-A1--D0-7--I2C+
       |   |   +-----------+-HREF-------+     |    |
      |   +-----------------D0-D7-------//8--+     |
      +---------------------I2C-SDA[27]SCL[28]A5--+
   -----------------------------------------------------------------*/
module   TinyFPGA_A2 (       				
				input 	IR,			// pin11 ->IR{A7)
				output	TP14,		//   pin14 ->SV2(A3)
				output	TP13,		// pin13 ->SV3(A2)
		
				output	SV0,		//   pin20 ->
				output	SV1,		// pin21 ->
				output	BUZZ,		// pin5   ->BUZZ(A6)
				output	RGB,		// pin8  ->RGB(D7)

                input   	XCLK2,   	// pin23 <-D9 16MHz			
				output	XCLK,		// pin4  ->cam   15MHz
				input	PCLK,		// pin9  <-cam
				output	DPCLK,		//   pin10 ->A0 =PCLK
				input	VSYNC,		// pin16 <-cam
				output	DVSYNC,		//   pin17 ->D8 =VSYNC
				input	HREF,		// pin12(A1) <-cam
			
				input   	RX,       	// pin25 <-D12(TX@HOST)
				output	TX			// pin26 ->D11(RX@HOST)   TXTP26
					);
 /***********************************************************************
   * CAMERA logic                                                         *
 ***********************************************************************/
   //reg [2:0] HAFC;  always @(posedge clk) HAFC <= HAFC + 1;
  //assign XCLK   = HAFC[0];			//       (15MHz)     [4] -> XCLK 
  assign DPCLK = PCLK;				//   PCLK ------> [9] [10] -> DPCLK(A0) 
  assign DVSYNC = VSYNC;			// VSYNC ----->   [16][17] -> DVSYNC(D8)
									// HREF -(A1)-> [12]
									//   XCLK(library)---> D9
  //assign XCLK = XCLK2;			// 16MHz
  assign XCLK   = CLK;				// 30MHz
  //assign TP14 = HREF;	// PIN14  (A3)
  //assign   TP13 = VSYNC;	// PIN13  (A2)
 /***********************************************************************
   * SYSTEM CLOCK  29.56(33.8)/26.60(37.6)/24.18(41.3)/22.17(45.1)/20.46(48.9)
   ***********************************************************************/	  
   wire CLK;  // base clock 29.56MHz 33.25ns
  OSCH #(.NOM_FREQ("29.56")) internal_oscillator_inst   (.STDBY(1'b0), .OSC(CLK)); // 
  assign clk = CLK;  
  /***********************************************************************
   * IR receiver                  32bit from MSB to LSB
 --|_9ms_start_|-4ms-|_600us_|-("0")600us-|_600us_|-("1")1600us-|_600us_stop_|--   
 ***********************************************************************/
   wire IRn; reg IRnn;			// IR pin/glidge canceller
  assign IR = IRn;				//   pin11

  reg [7:0] IRcntL, IRcntH;	// input signal glidge canceller
  always   @(posedge clk) begin 	// remove glidge
	if(IRn == 0) begin IRcntL <= IRcntL   + 1; IRcntH <= 0;	end
	if(IRn == 1) begin	IRcntH <= IRcntH + 1; IRcntL <=   0;	end
	if(IRcntL > 20) IRnn <= 0;    if(IRcntH > 20) IRnn <= 1;
  end
   
  reg [31:0] IRreg; 			// 32bit receive data buffer
  reg [7:0] rep;				//   decoded 8bit char data (to uart)
  reg [5:0] irseq; 			// ir receive sequence   
  
  reg [19:0] ctirL, ctirH; 	// l/h clock(33.8ns) counter 
  reg ena,   pena;				// enable gate flags
  reg Ureq;						// uart request
   always @(posedge clk) begin	
	// enable signal from Startbit && H
	if(ctirH   > 20'h30000) begin pena <= 0; ena <= 1'b0; irseq <= 0; 	end // idle 6.4ms
	if(ctirL   > 20'h30000) begin pena <= 1;        					end // prenable
	if(pena == 1   && IRnn == 1) begin ena <= 1'b1;         			end	// enable
	if(Ureq == 1   && TxD_data == 0) Ureq <= 1'b0; // reset uart transfer request @TxD started
	//   receive 32 bit data
	if(ena == 1) 	begin
	if(IRnn == 0) 	begin	// "L"   ---- 
		if(ctirL == 0) begin //
			if(irseq > 0) begin
			if(ctirH   > 18'h4557) 	begin IRreg[32-irseq[5:0]] = 1'b1; end // > 600us 
			else                 	begin   IRreg[32-irseq[5:0]] = 1'b0; end // < 600us		
			if(irseq == 32) begin 		//		
				Ureq   <= 1'b1; 	   		// Uart request pulse on
				rep <= IRreg[7:0]; 	// LSB   8bit
  				//case(IRreg[15:0]) // decode data 32bit >> char 8bit
				//	16'hA25D:   rep <= 8'h31; // "1(0x31)" 
				//	16'h629D: rep <= 8'h32; // "2(0x32)"   
				//	16'hE21D: rep <= 8'h33; // "3(0x33)" 
				//	16'h22DD:   rep <= 8'h34; // "4(0x34)" 
				//	16'h02FD: rep <= 8'h35; // "5(0x35)"   
				//	16'hC23D: rep <= 8'h36; // "6(0x36)" 
				//	16'hE01F:   rep <= 8'h37; // "7(0x37)" 
				//	16'hA857: rep <= 8'h38; // "8(0x38)"
				//	16'h906F:   rep <= 8'h39; // "9(0x39)" 
				//	16'h9867: rep <= 8'h30; // "0(0x30)"   
				//	16'h6897: rep <= 8'h2A; // "*(0x2A)" 
				//	16'hB04F:   rep <= 8'h23; // "#(0x23)" 
				//	16'h18E7: rep <= 8'h55; // "U(0x55)"   
				//	16'h4AB5: rep <= 8'h44; // "D(0x44)" 
				//	16'h10EF:   rep <= 8'h4C; // "L(0x4C)" 
				//	16'h5AA5: rep <= 8'h52; // "R(0x52)"   
				//	16'h38C7: rep <= 8'h4B; // "K(0x4B)" *"C(0x43)":SVO COMP
				//endcase			
			end
			end
		end
	end   //"H" ----	 next step
	else if(ctirH == 0) irseq <= irseq + 1; 	
	end
	//   L/H pulse width counters
	if(IRnn == 0) 	begin				// "L" ---- 
		if(ctirL   != 0) ctirH <= 0; 	// first entry after H >> L(counter == 0)
		if(ctirL <   20'h45000) ctirL <= ctirL + 1; 	// "L" timer count up	9ms
	end else  		begin				//   "H" ----	
		if(ctirH != 0) ctirL <= 0; 	// first entry after L >> H(counter   == 0)
		if(ctirH < 20'h45000) ctirH <= ctirH + 1; 	// "H" timer count up
	end
   end
 /***********************************************************************
   * UART Transfer    output TX[25]--D12 RX@HOST  input RX[23]--D9 TX@HOST         
   [IR command to UART command transfer]                
    -->Receive IR data   v     reset by TxL
	                __X-[Ureq]-X_ v^
					   Start Uart   Tx transfer
 ***********************************************************************/	
   //reg [10:0] BaudDivCnt;				// [10:0] 11bit 14400 tick 69.6(67.8us)us
   reg [7:0] BaudDivCnt;				// [7:0] 8bit 115200 tick 8.7(8.5us)us
  wire   BaudTick = (BaudDivCnt == 15); 	// 

  reg [7:0] Ustate;			// state machine
   reg TxD_start, TxD_data;		// data line
  reg [7:0] Udata;				// Transfer   data rep[7:0];
  assign TX = TxD_data;		// pin26 to D11

  always @(posedge   clk) begin
  BaudDivCnt <= BaudDivCnt + 1;
  if(Ustate == 0) TxD_data  <=   0'b1;  // preset
  //- receive transfer request -------------------------
   if(Ureq && TxD_start == 0) 	begin //receive req/preset
	  TxD_start <= 1'b1;   Ustate <= 8'h00; Udata <= rep; 	// load transfer data (IR data/Trans comp)
   end
  if(SCreq && TxD_start == 0) 	begin //servo comp req/preset
	  TxD_start   <= 1'b1; Ustate <= 8'h00; Udata <= 8'h43; // "C" servo comp report
  end
   //- state machine-------------------------------------
  if(TxD_start && BaudTick)   Ustate <= Ustate + 1;
  case(Ustate)
	 8'h03: TxD_data <= 0'b0;		// Start   bit
	 8'h04: TxD_data <= Udata[0];	// out bit0
	 8'h05: TxD_data <= Udata[1];	//   out bit1
	 8'h06: TxD_data <= Udata[2];	// out bit2
	 8'h07: TxD_data <=   Udata[3];	// out bit3
	 8'h08: TxD_data <= Udata[4];	// out bit4
	 8'h09:   TxD_data <= Udata[5];	// out bit5
	 8'h0A: TxD_data <= Udata[6];	// out bit6
	   8'h0B: TxD_data <= Udata[7];	// out bit7
	 8'h0C: TxD_data  <= 0'b1;		//   out stopbit
	 8'h0D: TxD_data  <= 0'b1;		// out stopbit
	 8'h0E: TxD_start   <= 1'b0; 	// complete and close
  endcase
  end
 /***********************************************************************
   * UART Receive     output TX[25]--D12 RX@HOST  input RX[23]--D9 TX@HOST         
	ff   no action/00 reset counter
 ***********************************************************************/
   //reg [9:0] BaudDivCntR; 					// [9:0] 10bit 14400 double tick
  reg [6:0]   BaudDivCntR;					// [6:0] 7bit 115200 tick 8.7us/2
  wire BaudTickR = (BaudDivCntR   == 15); 	//

  reg RxD_start;
  wire RxD_data = RX; 			// loop back   test
  reg [5:0] Ubp;				// buffer pointer 8x5byte 0-40
  reg [6:0] UstateR;			//   state machine
  reg [39:0] URdtR;			// 5bytex8 data
  reg monit;					//   debug flag
  reg [3:0] CMD;				// command reg trasfer command to servo control
   
  always @(posedge clk) begin
  BaudDivCntR <= BaudDivCntR + 1;
  if(RxD_data   == 0 && RxD_start == 0) begin 	//receive request/preset
	   BaudDivCntR <=   14; // Sync with received RxD_data start
	   RxD_start <= 1'b1;  UstateR <=   8'h00; 	// need reset source
	   //monit <= 0;
  end 	
			
  if(RxD_start   && BaudTickR) UstateR <= UstateR + 1;
  case(UstateR)
	 8'd3:  if(BaudTickR)   begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit0
	 8'd5:  if(BaudTickR)   begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit1
	 8'd7:  if(BaudTickR)   begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit2	 
	 8'd9:  if(BaudTickR)   begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit3
	 8'd11: if(BaudTickR)   begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit4
	 8'd13: if(BaudTickR)   begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit5	 	 
	 8'd15: if(BaudTickR)   begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit6
	 8'd17: if(BaudTickR)   begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit7
	 8'd19: if(BaudTickR)   begin	// stop bit, store to each registers
		if(Ubp == 8'd40) begin
			Ubp   <= 0; 				// Reset counter
			case(URdtR[7:0]) 		// byte0
				8'h01:   begin		// servo position command 0xff:do nothing
					CMD <= 8'h01;	//   set command received
					if(URdtR[15:8] != 8'hff)	chNSVO[0] <= URdtR[15:8];	//   SV0 target angle	 	
					if(URdtR[23:16] != 8'hff)	chNSVO[1] <= URdtR[23:16];	//   SV1 target angle
					if(URdtR[31:24] != 8'hff)	chNSVO[2] <= URdtR[31:24];	//   SV2 target angle	 	
					if(URdtR[39:32] != 8'hff)	chNSVO[3] <= URdtR[39:32];	//   SV3 target angle
					   end
				8'h02: begin		// servo speed command
					CMD   <= 8'h02;  // set command received
					if(URdtR[15:8] != 8'hff)  SVsped   <= URdtR[15:8];	 // wait counter for delay
					if(URdtR[23:16] != 8'hff)   SVstop <= URdtR[23:16]; // 0000xxxx target <= current
					else SVstop <=   0;									 // pause flag reset
					if(URdtR[31:24] != 8'hff)   SVswng <= URdtR[31:24]; // mode/depth(6)	
					   end
				8'h03:   begin 		// RGB led command
					RGBR[23:16] <= URdtR[23:16]-1;	// G
					RGBR[15:8]	<=   URdtR[15:8]-1; 		// R
					RGBR[7:0] 	<= URdtR[31:24]-1; 	// B
					    end
				8'h04: begin 		// BUZ command
					BuzNote <= URdtR[15:8];		//   0x96,97,98,99,9A,9B noTon 0x0f
					BuzLgth <= URdtR[23:16]; 		// Option   bit7:enable svo/led pulse
					   end
			endcase
		end
		else   begin // reset receive data buffer pointer
			if(		(Ubp == 8'd8) &&(URdtR[7:0]   	== 8'h00)	// closing @1st byte
				|| 	(Ubp == 8'd16)&&(URdtR[15:8]	==   8'h00)	// closing @2nd byte
				|| 	(Ubp == 8'd24)&&(URdtR[23:16]	== 8'h00)   	// closing @3rd byte
				|| 	(Ubp == 8'd32)&&(URdtR[31:24]	== 8'h00)   	// closing @4th byte
			) begin Ubp <= 0;  end // Reset counter
		end
	   end
	 8'd21: begin RxD_start <= 1'b0; CMD<= 0; end	// command received pulse   4us
  endcase
  end
  //--monitor-----------------------------------------
   //assign TP14 = svocpm[4];	// PIN14  (A3)
  //assign TP14 = monit;	// PIN14   (A3)
  //assign TP13 = RxD_data;		// PIN13  (A2)
  //--------------------------------------------------
   /***********************************************************************
 * Connection   REGISTER SET                      // xff:no load/0:no if  *
 ***********************************************************************/   
  reg [7:0] chNSVO[3:0];						// SERVO TARGET 0x01/byte1/byte2/byte3/byte4(1-0xfe)
   reg [7:0] SVsped, SVstop, SVswng; 	    	// SERVO SPEED 0x02/00010000(),wait/0001111(stp)
   reg [23:0] RGBR;								// RGB LED 0x03/red/green/blue()0x01-0xfe(-1)
   reg [7:0] BuzNote, BuzLgth;		 			// BUZ NOTE 0x04/0x96,97,98,99,9A,9B noTon   0x0f
 /***********************************************************************
   * SERVO controller     0.5ms---1.5ms---2.5ms                           *
      ____________   _________                                        ______
 ____|set         ^         |reset__________reset____//____reset___|set   ^ 
 con=0xAEFE  con=0xB4A2->0  con[11:2]==act[9:0] 0x0000-0x2d0(0-2ms) con=0xAEFE
	output	SV0,		//   pin20 ->
	output	SV1,		// pin21 ->
   	output	TP14,		// pin14 ->SV2(A3)
	output	TP13,		//   pin13 ->SV3(A2)
	BuzLgth[7] ebable pulse
	BuzLgth[6] ebable On track (stop   servo pulse/issue servo comp)
 ***********************************************************************/
   reg [3:0] svOUT;    	   						 	// output signalx4 lines
    assign   SV0  = svOUT[0];		// pin20
	assign SV1  = svOUT[1];		// pin21					
     assign TP14 = svOUT[2]; 		// pin14 SV2
    assign TP13 = svOUT[3]; 		//   pin13 SV3
  parameter CYCL = 20'h80000;						// 16ms Servo cycle
  parameter   PPAL = 16'h3f00;						// 0.5ms Pre Pulse    
  reg [19:0] svREG; 	     						//   Servo base counter
  
  reg [7:0] svTGT[3:0];							// 8bit target   register
  reg [9:0] svCRT[3:0];							// 10bit current count up
  reg   [9:0] svocpm; 								// set speed by uart
  reg onTrk;
  
  reg   [1:0] rot;								    // servo ch rotation
  always @(posedge clk) begin
	if(rot   == 0) rot <= 1; if(rot == 1) rot <= 2;
	if(rot == 2) rot <= 3; if(rot == 3)   rot <= 0;
  end
	
  always @(posedge clk) begin
	if(svREG > 17'h15000   && svREG < 19'h70000) begin // avoid servo positive pulse
	if(	{svTGT[0], 2'b00}   == svCRT[0] && {svTGT[1], 2'b00} == svCRT[1] &&
		{svTGT[2], 2'b00} == svCRT[2]   && {svTGT[3], 2'b00} == svCRT[3] && BuzLgth[6] == 1
	) onTrk <= 1; else onTrk   <= 0;
	end
  end
	
  always @(posedge clk) begin svREG <= svREG +   1; // timer count up
	if(BuzLgth[7] == 1) begin 
      if(onTrk == 0) begin
	   //-20bit-----4bit----10bit------6bit]------------------------
	  if(svREG   < {4'h0, svCRT[rot], 6'h0}) svOUT[rot] <= 1'b1; 			
	  else svOUT[rot] <=   1'b0; 						// PULSE GENERATE0	
	  if(svREG > CYCL - PPAL + 3000) svOUT   <= 4'hf;	// BASE 0.5ms PULSE				
	  if(svREG > CYCL) svREG <= 0; 					//   END CYCLE
	  end else svOUT <= 0;
	end
  end
 /***********************************************************************
   * us Event Control counter 29.56MHz(33.8ns)                            *
 ***********************************************************************/
   reg [14:0] cntU; reg uTick; parameter uDiv = 17750/3; 	// 29.56MHz(33.8ns) >>   2kHz(600us)  
  always @(posedge clk) uTick <= (cntU == uDiv-2);	   	// counter   reset pulse    
  always @(posedge clk) if(uTick) cntU <= 0; else cntU <= cntU   + 1; // |19.2|9.6|4.8|2.4|1.2|0.6ms|
 //---------------------------------------------------------------------
   reg [9:0] svo[3:0]; 						// wait control
  reg [9:0] svocp[3:0];						//   compare data delay

  always @(posedge clk) begin // INCREASE/DECREASE position   svCRT[rot]
	if(uTick) begin  // delay timer COUNT UP/DOWN
		svo[0] <= svo[0]   + 1'b1; svo[1] <= svo[1] + 1'b1;
		svo[2] <= svo[2] + 1'b1; svo[3] <= svo[3]   + 1'b1;
    end	
	if(svo[rot] > svocp[rot]) begin svo[rot] <= 10'h0;  		//   wait time
		if({svTGT[rot], 2'b00} > svCRT[rot]) svCRT[rot] <= svCRT[rot] +   1'b1; // increase
		if({svTGT[rot], 2'b00} < svCRT[rot]) svCRT[rot] <= svCRT[rot]   - 1'b1; // decrease			 
	end 
  end
  
  always @(posedge clk) begin   // SPEED UP by distance CONTROL  0---30(120)--60{240}--180(720) svocp[rot]
	if(svocpm[7]   == 0) begin // bit7:disable accel bit6-0:waiting value
	if({svTGT[rot], 2'b00}   > svCRT[rot]) begin 
		if({svTGT[rot], 2'b00} - svCRT[rot] > 240) 		svocp[rot]   <= {3'b00, svocpm[7:3]};
		else if({svTGT[rot], 2'b00} - svCRT[rot] < 100)   	svocp[rot] <= svocpm;
		else 												svocp[rot] <= {2'b00,   svocpm[7:2]}; 
	end // increase
	else if({svTGT[rot], 2'b00} < svCRT[rot])   begin 
		if(svCRT[rot] - {svTGT[rot], 2'b00} > 240) 		svocp[rot] <= {3'b00,   svocpm[7:3]};
		else if(svCRT[rot] - {svTGT[rot], 2'b00} < 100)	svocp[rot]   <= svocpm;
		else 												svocp[rot] <= {2'b00, svocpm[7:2]};   
	end // decrease 
	end else svocp[rot] <= svocpm[6:0]; // flat control
   end
/***********************************************************************
   * SERVO COMPLETE uart req                                             *
 ***********************************************************************/   
  reg SVCOMP;		// servo complete
  reg SCreq;		// servo comp report   request pulse
  reg [2:0] SMS; 	// state
  always @(posedge clk) begin SMS   <= SMS + 1;  
	if(swan[3:0] == 0) begin // SVswng[3:0] == 0 enable comp report
   	case(SMS)
		3'h01: begin
			if(onTrk) begin SVCOMP <= 1; if(SVCOMP   == 0) SCreq <= 1; end	// leading edge
			else SVCOMP <= 0; 
		end
		3'h06:   if(SCreq == 1) SCreq <= 0;
	endcase
	end
  end
 /***********************************************************************
   * ms Event Control LOAD COMMAND/BREATH move                            *
  svTGT[3:0],   svocpm	 // target/speed/stop bit INTERNAL
  chNSVO[3:0]			 // SERVO TARGET   0x01/byte1/byte2/byte3/byte4(1-0xfe)
  SVsped, SVstop, SVswng // SERVO SPEED   0x02/00010000(),wait/0001111(stp)
  [UART command to Servo execution]  
	-->Receive   UART command 5bytes
	                       X-[CMD]-Xv
                     load   to [catchCMD]v              ^ reset catch CMD 
					         load parameter   from command @mTicK
 ***********************************************************************/
   reg [14:0] cntM; reg mTick; parameter mDiv = 10000; 	// 400us 28000
  always   @(posedge clk) mTick <= (cntM == mDiv-2);		// counter reset pulse  
  always   @(posedge clk) if(mTick) cntM <= 0; else cntM <= cntM + 1'b1;  //
 //---------------------------------------------------------------------
   reg [11:0] led;  	// state machine
  reg [3:0]catchCMD;	// copy of uart command   reg CMD
  reg [7:0] swan;		// swing parameters polarity4bit + depth 4bit
   always @(posedge clk) begin
	  if(mTick && RxD_start == 0) begin led <= led   + 1'b1; 
	    case(led) // 1sec interval counter sequence control
		  default:   begin // load uart command every 400us
			      // access svTGT, sync with   CMD/catchCMD ontime after receiving uart command
				  // need 1ms interval   after command 0x01/0x02
				  if(catchCMD == 2'h01) begin 	// angle01/2/3
					   svTGT[0] <=  chNSVO[0];	// ch0 load target angle from uart
					 svTGT[1]   <= ~chNSVO[1];	// ch1
					 svTGT[2] <=  chNSVO[2];	// ch2
					   svTGT[3] <= ~chNSVO[3];	// ch3
					 catchCMD <= 0;  			// reset catched   command (ack)
					 monit <= 0;
				  end
				  if(catchCMD   == 2'h02) begin 	// speed/stop/swing
					 if(SVstop[0] == 1) svTGT[0][7:0]   <= svCRT[0][9:2]; 	// ch0
					 if(SVstop[1] == 1) svTGT[1][7:0] <= svCRT[1][9:2];   	// ch1
					 if(SVstop[2] == 1) svTGT[2][7:0] <= svCRT[2][9:2]; 	// ch2
					   if(SVstop[3] == 1) svTGT[3][7:0] <= svCRT[3][9:2]; 	// ch3
					 swan <=   SVswng;			//
					 //svocpm <= 8'b00010000; 	// set data from uart
					   svocpm <= SVsped;			// load speed reg from uart
				     catchCMD <= 0;   			// reset catched command (ack)
					 monit <= 1;
				  end
			     end
		  {4'b0100, 8'h00}: begin 
			    if(swan[3:0] != 0) begin
				   if(swan[4] == 0 && svTGT[0] > 32) svTGT[0] = svTGT[0] - {swan[3:0], 1'b0}; //front
				   else if(svTGT[0]< 223) svTGT[0] = svTGT[0] + {swan[3:0], 1'b0};
				  if(swan[5]   == 0 && svTGT[1] > 32) svTGT[1] = svTGT[1] - {swan[3:0], 1'b0}; 
				  else   if(svTGT[1]< 223) svTGT[1] = svTGT[1] + {SVswng[3:0], 1'b0};
			//	  if(swan[6]   == 0 && svTGT[2] > 32) svTGT[2] = svTGT[2] - {swan[3:0], 1'b0}; // rear
			//	   else if(svTGT[2]< 223) svTGT[2] = svTGT[2] + {SVswng[3:0], 1'b0}; 
			//	   if(swan[7] == 0 && svTGT[3] > 32) svTGT[3] = svTGT[3] - {swan[3:0], 1'b0}; 
			//	   else if(svTGT[3]< 223) svTGT[3] = svTGT[3] + {swan[3:0], 1'b0}; 
				end
				end   								//
		  {4'b1100, 8'h00}: begin 
			    if(swan[3:0] !=   0) begin
				  if(swan[4] != 0 && svTGT[0] > 32) svTGT[0] = svTGT[0] - {swan[3:0],   1'b0}; // front
				  else if(svTGT[0]< 223) svTGT[0] = svTGT[0] + {swan[3:0],   1'b0};
				  if(swan[5] != 0 && svTGT[1] > 32) svTGT[1] = svTGT[1] - {swan[3:0],   1'b0}; 
				  else if(svTGT[1]< 223) svTGT[1] = svTGT[1] + {swan[3:0], 1'b0};
			//	   if(swan[6] != 0 && svTGT[2] > 32) svTGT[2] = svTGT[2] - {swan[3:0], 1'b0}; //   rear
			//	  else if(svTGT[2]< 223) svTGT[2] = svTGT[2] + {swan[3:0], 1'b0};
			//	   if(swan[7] != 0 && svTGT[3] > 32) svTGT[3] = svTGT[3] - {swan[3:0], 1'b0}; 
			//	   else if(svTGT[3]< 223) svTGT[3] = svTGT[3] + {swan[3:0], 1'b0};
				end
				end   
	    endcase
	  end else begin if(CMD != 0) begin catchCMD <= CMD; end   // catch 4us 0x01/0x02 command pulse
	  end // catch uart command
  end
   /***********************************************************************
 * RGB   LED controller  G-R-B 24bit [11:8](RGB+5byte blank) [7:5](byte cntr) [4:0](0(10/22)
   _|-2-500ns-|__650--950ns_____|"0" _|--550--650ns-|__450--750ns_|"1" -|_//_>50us_|-   "reset"  
  BuzLgth[7] ebable pulse 
 ***********************************************************************/
   reg rgbOUT;    	    // output signal
  assign RGB = rgbOUT;	// [8] D7
   reg [23:0] RGBreg;	// 3byte RGB color set through uart
  reg [21:0] ctRGB;	//   rgb clock(33.25ns) 21-8(byte pointer)/9-5(24bit pointer)
  reg [4:0] dptn; 		//   data pattern "0/1" compare reg
  always @(posedge clk) begin ctRGB <= ctRGB   + 1;
	  RGBreg <= RGBR;	// RGBR[23:0]: LED 0x03/red/green/blue()0x01-0xfe(-1)
	   if(BuzLgth[7] == 1) begin
	  if(ctRGB[21:8] == 0 || ctRGB[21:8] == 1 || ctRGB[21:8]   == 2) begin // idle @24bit~
		  if(RGBreg[23-ctRGB[9:5]] == 0)	dptn <= 5'b01010;   	// "0" rgbout = 1'b1; 360/800
		  else 								dptn <= 5'b10001;   	// "1" rgbout = 1'b1; 580/600
		  if(ctRGB[4:0] < dptn[4:0]) rgbOUT <=   1; else rgbOUT <= 0;
	  end else rgbOUT = 1'b0;     							// "0" reset   mode
	 end
  end
 /***********************************************************************
   * BUZZ controll BuzNote[7:0], BuzLgth[7:0]; // BUZ NOTE 0x04/0x96,97,98,99,9A,9B   noTon 0x0f
 ***********************************************************************/
   reg speaker;				// sound pulse
  assign BUZZ = speaker;	// [5] A6
   reg [27:0] tone;
  reg [7:0] ntype; 		// 0x96,97,98,99,9A,9B/0x0f dis
   always @(posedge clk) begin ntype <= BuzNote; end
  always @(posedge clk) begin   tone <= tone+1; end
  wire [6:0] fastsweep = (tone[22] ? tone[21:15] : ~tone[21:15]);
   wire [6:0] slowsweep = (tone[25] ? tone[24:18] : ~tone[24:18]);
  wire [14:0]   clkdivider = {2'b01, (tone[ntype[6:0]] ? slowsweep : fastsweep), 6'b000000}; 

   reg [14:0] counter;
  always @(posedge clk) if(counter==0) counter <= clkdivider;   else counter <= counter-1;
  always @(posedge clk) if(counter==0 && ntype[7])   speaker <= ~speaker;
	  
endmodule

FPGA verilog

verilog

1/* ------------------------------------------------------------------
2   FPGA code
3   MEDAMA PROJECT
4   PORT/FPGApin  ASSIGNMENT
5                     +-fpga-----+
6                     |MACHXO2   8--->RGB(D7)
7           (A7)IR--->11         5--->BUZ(A6)
8                     |         20--->SV0
9                     |         21--->SV1
10                     |         14--->SV2(A3) can use for monitor
11                     |         13--->SV3(A2) can use for monitor
12    +-cam-------+    |          |    +-mpu----------+
13    |OV767X     |    |   XCLK2 23<---D9  XCLK nano33|
14    |           |    |      RX 25<---D12 TX   3.3V  |
15    |        XCLK<---4  XCLK TX 26--->D11 RX         |
16    |           |    |          |    |              |
17    |        PCLK--->9 PCLK    10--->A0 DPCLK       |
18    |       VSYNC--->16  VSYNC  17--->D8 DVSYNC  A4/A5
19    I2C-BUS-HREF+    +---12-----+    +-A1--D0-7--I2C+
20      |   |   +-----------+-HREF-------+     |    |
21      |   +-----------------D0-D7-------//8--+    |
22      +---------------------I2C-SDA[27]SCL[28]A5--+
23   -----------------------------------------------------------------*/
24module  TinyFPGA_A2 (       				
25				input 	IR,			// pin11 ->IR{A7)
26				output	TP14,		//  pin14 ->SV2(A3)
27				output	TP13,		// pin13 ->SV3(A2)
28		
29				output	SV0,		//  pin20 ->
30				output	SV1,		// pin21 ->
31				output	BUZZ,		// pin5  ->BUZZ(A6)
32				output	RGB,		// pin8  ->RGB(D7)
33
34                input  	XCLK2,   	// pin23 <-D9 16MHz			
35				output	XCLK,		// pin4  ->cam  15MHz
36				input	PCLK,		// pin9  <-cam
37				output	DPCLK,		//  pin10 ->A0 =PCLK
38				input	VSYNC,		// pin16 <-cam
39				output	DVSYNC,		//  pin17 ->D8 =VSYNC
40				input	HREF,		// pin12(A1) <-cam
41			
42				input  	RX,       	// pin25 <-D12(TX@HOST)
43				output	TX			// pin26 ->D11(RX@HOST)  TXTP26
44					);
45 /***********************************************************************
46  * CAMERA logic                                                         *
47 ***********************************************************************/
48  //reg [2:0] HAFC;  always @(posedge clk) HAFC <= HAFC + 1;
49  //assign XCLK  = HAFC[0];			//       (15MHz)     [4] -> XCLK 
50  assign DPCLK = PCLK;				//  PCLK ------> [9] [10] -> DPCLK(A0) 
51  assign DVSYNC = VSYNC;			// VSYNC ----->  [16][17] -> DVSYNC(D8)
52									// HREF -(A1)-> [12]
53									//  XCLK(library)---> D9
54  //assign XCLK = XCLK2;			// 16MHz
55  assign XCLK  = CLK;				// 30MHz
56  //assign TP14 = HREF;	// PIN14  (A3)
57  //assign  TP13 = VSYNC;	// PIN13  (A2)
58 /***********************************************************************
59  * SYSTEM CLOCK  29.56(33.8)/26.60(37.6)/24.18(41.3)/22.17(45.1)/20.46(48.9)
60  ***********************************************************************/	  
61  wire CLK;  // base clock 29.56MHz 33.25ns
62  OSCH #(.NOM_FREQ("29.56")) internal_oscillator_inst  (.STDBY(1'b0), .OSC(CLK)); // 
63  assign clk = CLK;  
64  /***********************************************************************
65  * IR receiver                  32bit from MSB to LSB
66 --|_9ms_start_|-4ms-|_600us_|-("0")600us-|_600us_|-("1")1600us-|_600us_stop_|--  
67 ***********************************************************************/
68  wire IRn; reg IRnn;			// IR pin/glidge canceller
69  assign IR = IRn;				//  pin11
70
71  reg [7:0] IRcntL, IRcntH;	// input signal glidge canceller
72  always  @(posedge clk) begin 	// remove glidge
73	if(IRn == 0) begin IRcntL <= IRcntL  + 1; IRcntH <= 0;	end
74	if(IRn == 1) begin	IRcntH <= IRcntH + 1; IRcntL <=  0;	end
75	if(IRcntL > 20) IRnn <= 0;    if(IRcntH > 20) IRnn <= 1;
76  end
77  
78  reg [31:0] IRreg; 			// 32bit receive data buffer
79  reg [7:0] rep;				//  decoded 8bit char data (to uart)
80  reg [5:0] irseq; 			// ir receive sequence  
81  
82  reg [19:0] ctirL, ctirH; 	// l/h clock(33.8ns) counter 
83  reg ena,  pena;				// enable gate flags
84  reg Ureq;						// uart request
85  always @(posedge clk) begin	
86	// enable signal from Startbit && H
87	if(ctirH  > 20'h30000) begin pena <= 0; ena <= 1'b0; irseq <= 0; 	end // idle 6.4ms
88	if(ctirL  > 20'h30000) begin pena <= 1;        					end // prenable
89	if(pena == 1  && IRnn == 1) begin ena <= 1'b1;         			end	// enable
90	if(Ureq == 1  && TxD_data == 0) Ureq <= 1'b0; // reset uart transfer request @TxD started
91	//  receive 32 bit data
92	if(ena == 1) 	begin
93	if(IRnn == 0) 	begin	// "L"  ---- 
94		if(ctirL == 0) begin //
95			if(irseq > 0) begin
96			if(ctirH  > 18'h4557) 	begin IRreg[32-irseq[5:0]] = 1'b1; end // > 600us 
97			else                 	begin  IRreg[32-irseq[5:0]] = 1'b0; end // < 600us		
98			if(irseq == 32) begin 		//		
99				Ureq  <= 1'b1; 	   		// Uart request pulse on
100				rep <= IRreg[7:0]; 	// LSB  8bit
101  				//case(IRreg[15:0]) // decode data 32bit >> char 8bit
102				//	16'hA25D:  rep <= 8'h31; // "1(0x31)" 
103				//	16'h629D: rep <= 8'h32; // "2(0x32)"  
104				//	16'hE21D: rep <= 8'h33; // "3(0x33)" 
105				//	16'h22DD:  rep <= 8'h34; // "4(0x34)" 
106				//	16'h02FD: rep <= 8'h35; // "5(0x35)"  
107				//	16'hC23D: rep <= 8'h36; // "6(0x36)" 
108				//	16'hE01F:  rep <= 8'h37; // "7(0x37)" 
109				//	16'hA857: rep <= 8'h38; // "8(0x38)"
110				//	16'h906F:  rep <= 8'h39; // "9(0x39)" 
111				//	16'h9867: rep <= 8'h30; // "0(0x30)"  
112				//	16'h6897: rep <= 8'h2A; // "*(0x2A)" 
113				//	16'hB04F:  rep <= 8'h23; // "#(0x23)" 
114				//	16'h18E7: rep <= 8'h55; // "U(0x55)"  
115				//	16'h4AB5: rep <= 8'h44; // "D(0x44)" 
116				//	16'h10EF:  rep <= 8'h4C; // "L(0x4C)" 
117				//	16'h5AA5: rep <= 8'h52; // "R(0x52)"  
118				//	16'h38C7: rep <= 8'h4B; // "K(0x4B)" *"C(0x43)":SVO COMP
119				//endcase			
120			end
121			end
122		end
123	end  //"H" ----	 next step
124	else if(ctirH == 0) irseq <= irseq + 1; 	
125	end
126	//  L/H pulse width counters
127	if(IRnn == 0) 	begin				// "L" ---- 
128		if(ctirL  != 0) ctirH <= 0; 	// first entry after H >> L(counter == 0)
129		if(ctirL <  20'h45000) ctirL <= ctirL + 1; 	// "L" timer count up	9ms
130	end else  		begin				//  "H" ----	
131		if(ctirH != 0) ctirL <= 0; 	// first entry after L >> H(counter  == 0)
132		if(ctirH < 20'h45000) ctirH <= ctirH + 1; 	// "H" timer count up
133	end
134  end
135 /***********************************************************************
136  * UART Transfer    output TX[25]--D12 RX@HOST  input RX[23]--D9 TX@HOST         
137  [IR command to UART command transfer]                
138    -->Receive IR data  v     reset by TxL
139	                __X-[Ureq]-X_ v^
140					   Start Uart  Tx transfer
141 ***********************************************************************/	
142  //reg [10:0] BaudDivCnt;				// [10:0] 11bit 14400 tick 69.6(67.8us)us
143  reg [7:0] BaudDivCnt;				// [7:0] 8bit 115200 tick 8.7(8.5us)us
144  wire  BaudTick = (BaudDivCnt == 15); 	// 
145
146  reg [7:0] Ustate;			// state machine
147  reg TxD_start, TxD_data;		// data line
148  reg [7:0] Udata;				// Transfer  data rep[7:0];
149  assign TX = TxD_data;		// pin26 to D11
150
151  always @(posedge  clk) begin
152  BaudDivCnt <= BaudDivCnt + 1;
153  if(Ustate == 0) TxD_data  <=  0'b1;  // preset
154  //- receive transfer request -------------------------
155  if(Ureq && TxD_start == 0) 	begin //receive req/preset
156	  TxD_start <= 1'b1;  Ustate <= 8'h00; Udata <= rep; 	// load transfer data (IR data/Trans comp)
157  end
158  if(SCreq && TxD_start == 0) 	begin //servo comp req/preset
159	  TxD_start  <= 1'b1; Ustate <= 8'h00; Udata <= 8'h43; // "C" servo comp report
160  end
161  //- state machine-------------------------------------
162  if(TxD_start && BaudTick)  Ustate <= Ustate + 1;
163  case(Ustate)
164	 8'h03: TxD_data <= 0'b0;		// Start  bit
165	 8'h04: TxD_data <= Udata[0];	// out bit0
166	 8'h05: TxD_data <= Udata[1];	//  out bit1
167	 8'h06: TxD_data <= Udata[2];	// out bit2
168	 8'h07: TxD_data <=  Udata[3];	// out bit3
169	 8'h08: TxD_data <= Udata[4];	// out bit4
170	 8'h09:  TxD_data <= Udata[5];	// out bit5
171	 8'h0A: TxD_data <= Udata[6];	// out bit6
172	  8'h0B: TxD_data <= Udata[7];	// out bit7
173	 8'h0C: TxD_data  <= 0'b1;		//  out stopbit
174	 8'h0D: TxD_data  <= 0'b1;		// out stopbit
175	 8'h0E: TxD_start  <= 1'b0; 	// complete and close
176  endcase
177  end
178 /***********************************************************************
179  * UART Receive     output TX[25]--D12 RX@HOST  input RX[23]--D9 TX@HOST         
180	ff  no action/00 reset counter
181 ***********************************************************************/
182  //reg [9:0] BaudDivCntR; 					// [9:0] 10bit 14400 double tick
183  reg [6:0]  BaudDivCntR;					// [6:0] 7bit 115200 tick 8.7us/2
184  wire BaudTickR = (BaudDivCntR  == 15); 	//
185
186  reg RxD_start;
187  wire RxD_data = RX; 			// loop back  test
188  reg [5:0] Ubp;				// buffer pointer 8x5byte 0-40
189  reg [6:0] UstateR;			//  state machine
190  reg [39:0] URdtR;			// 5bytex8 data
191  reg monit;					//  debug flag
192  reg [3:0] CMD;				// command reg trasfer command to servo control
193  
194  always @(posedge clk) begin
195  BaudDivCntR <= BaudDivCntR + 1;
196  if(RxD_data  == 0 && RxD_start == 0) begin 	//receive request/preset
197	   BaudDivCntR <=  14; // Sync with received RxD_data start
198	   RxD_start <= 1'b1;  UstateR <=  8'h00; 	// need reset source
199	   //monit <= 0;
200  end 	
201			
202  if(RxD_start  && BaudTickR) UstateR <= UstateR + 1;
203  case(UstateR)
204	 8'd3:  if(BaudTickR)  begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit0
205	 8'd5:  if(BaudTickR)  begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit1
206	 8'd7:  if(BaudTickR)  begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit2	 
207	 8'd9:  if(BaudTickR)  begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit3
208	 8'd11: if(BaudTickR)  begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit4
209	 8'd13: if(BaudTickR)  begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit5	 	 
210	 8'd15: if(BaudTickR)  begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit6
211	 8'd17: if(BaudTickR)  begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit7
212	 8'd19: if(BaudTickR)  begin	// stop bit, store to each registers
213		if(Ubp == 8'd40) begin
214			Ubp  <= 0; 				// Reset counter
215			case(URdtR[7:0]) 		// byte0
216				8'h01:  begin		// servo position command 0xff:do nothing
217					CMD <= 8'h01;	//  set command received
218					if(URdtR[15:8] != 8'hff)	chNSVO[0] <= URdtR[15:8];	//  SV0 target angle	 	
219					if(URdtR[23:16] != 8'hff)	chNSVO[1] <= URdtR[23:16];	//  SV1 target angle
220					if(URdtR[31:24] != 8'hff)	chNSVO[2] <= URdtR[31:24];	//  SV2 target angle	 	
221					if(URdtR[39:32] != 8'hff)	chNSVO[3] <= URdtR[39:32];	//  SV3 target angle
222					   end
223				8'h02: begin		// servo speed command
224					CMD  <= 8'h02;  // set command received
225					if(URdtR[15:8] != 8'hff)  SVsped  <= URdtR[15:8];	 // wait counter for delay
226					if(URdtR[23:16] != 8'hff)  SVstop <= URdtR[23:16]; // 0000xxxx target <= current
227					else SVstop <=  0;									 // pause flag reset
228					if(URdtR[31:24] != 8'hff)  SVswng <= URdtR[31:24]; // mode/depth(6)	
229					   end
230				8'h03:  begin 		// RGB led command
231					RGBR[23:16] <= URdtR[23:16]-1;	// G
232					RGBR[15:8]	<=  URdtR[15:8]-1; 		// R
233					RGBR[7:0] 	<= URdtR[31:24]-1; 	// B
234					   end
235				8'h04: begin 		// BUZ command
236					BuzNote <= URdtR[15:8];		//  0x96,97,98,99,9A,9B noTon 0x0f
237					BuzLgth <= URdtR[23:16]; 		// Option  bit7:enable svo/led pulse
238					   end
239			endcase
240		end
241		else  begin // reset receive data buffer pointer
242			if(		(Ubp == 8'd8) &&(URdtR[7:0]  	== 8'h00)	// closing @1st byte
243				|| 	(Ubp == 8'd16)&&(URdtR[15:8]	==  8'h00)	// closing @2nd byte
244				|| 	(Ubp == 8'd24)&&(URdtR[23:16]	== 8'h00)  	// closing @3rd byte
245				|| 	(Ubp == 8'd32)&&(URdtR[31:24]	== 8'h00)  	// closing @4th byte
246			) begin Ubp <= 0;  end // Reset counter
247		end
248	  end
249	 8'd21: begin RxD_start <= 1'b0; CMD<= 0; end	// command received pulse  4us
250  endcase
251  end
252  //--monitor-----------------------------------------
253  //assign TP14 = svocpm[4];	// PIN14  (A3)
254  //assign TP14 = monit;	// PIN14  (A3)
255  //assign TP13 = RxD_data;		// PIN13  (A2)
256  //--------------------------------------------------
257  /***********************************************************************
258 * Connection  REGISTER SET                      // xff:no load/0:no if  *
259 ***********************************************************************/  
260  reg [7:0] chNSVO[3:0];						// SERVO TARGET 0x01/byte1/byte2/byte3/byte4(1-0xfe)
261  reg [7:0] SVsped, SVstop, SVswng; 	    	// SERVO SPEED 0x02/00010000(),wait/0001111(stp)
262  reg [23:0] RGBR;								// RGB LED 0x03/red/green/blue()0x01-0xfe(-1)
263  reg [7:0] BuzNote, BuzLgth;		 			// BUZ NOTE 0x04/0x96,97,98,99,9A,9B noTon  0x0f
264 /***********************************************************************
265  * SERVO controller     0.5ms---1.5ms---2.5ms                           *
266      ____________  _________                                        ______
267 ____|set         ^         |reset__________reset____//____reset___|set  ^ 
268 con=0xAEFE  con=0xB4A2->0  con[11:2]==act[9:0] 0x0000-0x2d0(0-2ms) con=0xAEFE
269	output	SV0,		//  pin20 ->
270	output	SV1,		// pin21 ->
271   	output	TP14,		// pin14 ->SV2(A3)
272	output	TP13,		//  pin13 ->SV3(A2)
273	BuzLgth[7] ebable pulse
274	BuzLgth[6] ebable On track (stop  servo pulse/issue servo comp)
275 ***********************************************************************/
276  reg [3:0] svOUT;    	   						 	// output signalx4 lines
277    assign  SV0  = svOUT[0];		// pin20
278	assign SV1  = svOUT[1];		// pin21					
279    assign TP14 = svOUT[2]; 		// pin14 SV2
280    assign TP13 = svOUT[3]; 		//  pin13 SV3
281  parameter CYCL = 20'h80000;						// 16ms Servo cycle
282  parameter  PPAL = 16'h3f00;						// 0.5ms Pre Pulse    
283  reg [19:0] svREG; 	     						//  Servo base counter
284  
285  reg [7:0] svTGT[3:0];							// 8bit target  register
286  reg [9:0] svCRT[3:0];							// 10bit current count up
287  reg  [9:0] svocpm; 								// set speed by uart
288  reg onTrk;
289  
290  reg  [1:0] rot;								    // servo ch rotation
291  always @(posedge clk) begin
292	if(rot  == 0) rot <= 1; if(rot == 1) rot <= 2;
293	if(rot == 2) rot <= 3; if(rot == 3)  rot <= 0;
294  end
295	
296  always @(posedge clk) begin
297	if(svREG > 17'h15000  && svREG < 19'h70000) begin // avoid servo positive pulse
298	if(	{svTGT[0], 2'b00}  == svCRT[0] && {svTGT[1], 2'b00} == svCRT[1] &&
299		{svTGT[2], 2'b00} == svCRT[2]  && {svTGT[3], 2'b00} == svCRT[3] && BuzLgth[6] == 1
300	) onTrk <= 1; else onTrk  <= 0;
301	end
302  end
303	
304  always @(posedge clk) begin svREG <= svREG +  1; // timer count up
305	if(BuzLgth[7] == 1) begin 
306      if(onTrk == 0) begin
307	  //-20bit-----4bit----10bit------6bit]------------------------
308	  if(svREG  < {4'h0, svCRT[rot], 6'h0}) svOUT[rot] <= 1'b1; 			
309	  else svOUT[rot] <=  1'b0; 						// PULSE GENERATE0	
310	  if(svREG > CYCL - PPAL + 3000) svOUT  <= 4'hf;	// BASE 0.5ms PULSE				
311	  if(svREG > CYCL) svREG <= 0; 					//  END CYCLE
312	  end else svOUT <= 0;
313	end
314  end
315 /***********************************************************************
316  * us Event Control counter 29.56MHz(33.8ns)                            *
317 ***********************************************************************/
318  reg [14:0] cntU; reg uTick; parameter uDiv = 17750/3; 	// 29.56MHz(33.8ns) >>  2kHz(600us)  
319  always @(posedge clk) uTick <= (cntU == uDiv-2);	   	// counter  reset pulse    
320  always @(posedge clk) if(uTick) cntU <= 0; else cntU <= cntU  + 1; // |19.2|9.6|4.8|2.4|1.2|0.6ms|
321 //---------------------------------------------------------------------
322  reg [9:0] svo[3:0]; 						// wait control
323  reg [9:0] svocp[3:0];						//  compare data delay
324
325  always @(posedge clk) begin // INCREASE/DECREASE position  svCRT[rot]
326	if(uTick) begin  // delay timer COUNT UP/DOWN
327		svo[0] <= svo[0]  + 1'b1; svo[1] <= svo[1] + 1'b1;
328		svo[2] <= svo[2] + 1'b1; svo[3] <= svo[3]  + 1'b1;
329    end	
330	if(svo[rot] > svocp[rot]) begin svo[rot] <= 10'h0;  		//  wait time
331		if({svTGT[rot], 2'b00} > svCRT[rot]) svCRT[rot] <= svCRT[rot] +  1'b1; // increase
332		if({svTGT[rot], 2'b00} < svCRT[rot]) svCRT[rot] <= svCRT[rot]  - 1'b1; // decrease			 
333	end 
334  end
335  
336  always @(posedge clk) begin  // SPEED UP by distance CONTROL  0---30(120)--60{240}--180(720) svocp[rot]
337	if(svocpm[7]  == 0) begin // bit7:disable accel bit6-0:waiting value
338	if({svTGT[rot], 2'b00}  > svCRT[rot]) begin 
339		if({svTGT[rot], 2'b00} - svCRT[rot] > 240) 		svocp[rot]  <= {3'b00, svocpm[7:3]};
340		else if({svTGT[rot], 2'b00} - svCRT[rot] < 100)  	svocp[rot] <= svocpm;
341		else 												svocp[rot] <= {2'b00,  svocpm[7:2]}; 
342	end // increase
343	else if({svTGT[rot], 2'b00} < svCRT[rot])  begin 
344		if(svCRT[rot] - {svTGT[rot], 2'b00} > 240) 		svocp[rot] <= {3'b00,  svocpm[7:3]};
345		else if(svCRT[rot] - {svTGT[rot], 2'b00} < 100)	svocp[rot]  <= svocpm;
346		else 												svocp[rot] <= {2'b00, svocpm[7:2]};  
347	end // decrease 
348	end else svocp[rot] <= svocpm[6:0]; // flat control
349  end
350/***********************************************************************
351  * SERVO COMPLETE uart req                                             *
352 ***********************************************************************/  
353  reg SVCOMP;		// servo complete
354  reg SCreq;		// servo comp report  request pulse
355  reg [2:0] SMS; 	// state
356  always @(posedge clk) begin SMS  <= SMS + 1;  
357	if(swan[3:0] == 0) begin // SVswng[3:0] == 0 enable comp report
358  	case(SMS)
359		3'h01: begin
360			if(onTrk) begin SVCOMP <= 1; if(SVCOMP  == 0) SCreq <= 1; end	// leading edge
361			else SVCOMP <= 0; 
362		end
363		3'h06:  if(SCreq == 1) SCreq <= 0;
364	endcase
365	end
366  end
367 /***********************************************************************
368  * ms Event Control LOAD COMMAND/BREATH move                            *
369  svTGT[3:0],  svocpm	 // target/speed/stop bit INTERNAL
370  chNSVO[3:0]			 // SERVO TARGET  0x01/byte1/byte2/byte3/byte4(1-0xfe)
371  SVsped, SVstop, SVswng // SERVO SPEED  0x02/00010000(),wait/0001111(stp)
372  [UART command to Servo execution]  
373	-->Receive  UART command 5bytes
374	                       X-[CMD]-Xv
375                     load  to [catchCMD]v              ^ reset catch CMD 
376					         load parameter  from command @mTicK
377 ***********************************************************************/
378  reg [14:0] cntM; reg mTick; parameter mDiv = 10000; 	// 400us 28000
379  always  @(posedge clk) mTick <= (cntM == mDiv-2);		// counter reset pulse  
380  always  @(posedge clk) if(mTick) cntM <= 0; else cntM <= cntM + 1'b1;  //
381 //---------------------------------------------------------------------
382  reg [11:0] led;  	// state machine
383  reg [3:0]catchCMD;	// copy of uart command  reg CMD
384  reg [7:0] swan;		// swing parameters polarity4bit + depth 4bit
385  always @(posedge clk) begin
386	  if(mTick && RxD_start == 0) begin led <= led  + 1'b1; 
387	    case(led) // 1sec interval counter sequence control
388		  default:  begin // load uart command every 400us
389			      // access svTGT, sync with  CMD/catchCMD ontime after receiving uart command
390				  // need 1ms interval  after command 0x01/0x02
391				  if(catchCMD == 2'h01) begin 	// angle01/2/3
392					  svTGT[0] <=  chNSVO[0];	// ch0 load target angle from uart
393					 svTGT[1]  <= ~chNSVO[1];	// ch1
394					 svTGT[2] <=  chNSVO[2];	// ch2
395					  svTGT[3] <= ~chNSVO[3];	// ch3
396					 catchCMD <= 0;  			// reset catched  command (ack)
397					 monit <= 0;
398				  end
399				  if(catchCMD  == 2'h02) begin 	// speed/stop/swing
400					 if(SVstop[0] == 1) svTGT[0][7:0]  <= svCRT[0][9:2]; 	// ch0
401					 if(SVstop[1] == 1) svTGT[1][7:0] <= svCRT[1][9:2];  	// ch1
402					 if(SVstop[2] == 1) svTGT[2][7:0] <= svCRT[2][9:2]; 	// ch2
403					  if(SVstop[3] == 1) svTGT[3][7:0] <= svCRT[3][9:2]; 	// ch3
404					 swan <=  SVswng;			//
405					 //svocpm <= 8'b00010000; 	// set data from uart
406					  svocpm <= SVsped;			// load speed reg from uart
407				     catchCMD <= 0;  			// reset catched command (ack)
408					 monit <= 1;
409				  end
410			    end
411		  {4'b0100, 8'h00}: begin 
412			    if(swan[3:0] != 0) begin
413				  if(swan[4] == 0 && svTGT[0] > 32) svTGT[0] = svTGT[0] - {swan[3:0], 1'b0}; //front
414				  else if(svTGT[0]< 223) svTGT[0] = svTGT[0] + {swan[3:0], 1'b0};
415				  if(swan[5]  == 0 && svTGT[1] > 32) svTGT[1] = svTGT[1] - {swan[3:0], 1'b0}; 
416				  else  if(svTGT[1]< 223) svTGT[1] = svTGT[1] + {SVswng[3:0], 1'b0};
417			//	  if(swan[6]  == 0 && svTGT[2] > 32) svTGT[2] = svTGT[2] - {swan[3:0], 1'b0}; // rear
418			//	  else if(svTGT[2]< 223) svTGT[2] = svTGT[2] + {SVswng[3:0], 1'b0}; 
419			//	  if(swan[7] == 0 && svTGT[3] > 32) svTGT[3] = svTGT[3] - {swan[3:0], 1'b0}; 
420			//	  else if(svTGT[3]< 223) svTGT[3] = svTGT[3] + {swan[3:0], 1'b0}; 
421				end
422				end  								//
423		  {4'b1100, 8'h00}: begin 
424			    if(swan[3:0] !=  0) begin
425				  if(swan[4] != 0 && svTGT[0] > 32) svTGT[0] = svTGT[0] - {swan[3:0],  1'b0}; // front
426				  else if(svTGT[0]< 223) svTGT[0] = svTGT[0] + {swan[3:0],  1'b0};
427				  if(swan[5] != 0 && svTGT[1] > 32) svTGT[1] = svTGT[1] - {swan[3:0],  1'b0}; 
428				  else if(svTGT[1]< 223) svTGT[1] = svTGT[1] + {swan[3:0], 1'b0};
429			//	  if(swan[6] != 0 && svTGT[2] > 32) svTGT[2] = svTGT[2] - {swan[3:0], 1'b0}; //  rear
430			//	  else if(svTGT[2]< 223) svTGT[2] = svTGT[2] + {swan[3:0], 1'b0};
431			//	  if(swan[7] != 0 && svTGT[3] > 32) svTGT[3] = svTGT[3] - {swan[3:0], 1'b0}; 
432			//	  else if(svTGT[3]< 223) svTGT[3] = svTGT[3] + {swan[3:0], 1'b0};
433				end
434				end  
435	    endcase
436	  end else begin if(CMD != 0) begin catchCMD <= CMD; end  // catch 4us 0x01/0x02 command pulse
437	  end // catch uart command
438  end
439  /***********************************************************************
440 * RGB  LED controller  G-R-B 24bit [11:8](RGB+5byte blank) [7:5](byte cntr) [4:0](0(10/22)
441  _|-2-500ns-|__650--950ns_____|"0" _|--550--650ns-|__450--750ns_|"1" -|_//_>50us_|-  "reset"  
442  BuzLgth[7] ebable pulse 
443 ***********************************************************************/
444  reg rgbOUT;    	    // output signal
445  assign RGB = rgbOUT;	// [8] D7
446  reg [23:0] RGBreg;	// 3byte RGB color set through uart
447  reg [21:0] ctRGB;	//  rgb clock(33.25ns) 21-8(byte pointer)/9-5(24bit pointer)
448  reg [4:0] dptn; 		//  data pattern "0/1" compare reg
449  always @(posedge clk) begin ctRGB <= ctRGB  + 1;
450	  RGBreg <= RGBR;	// RGBR[23:0]: LED 0x03/red/green/blue()0x01-0xfe(-1)
451	  if(BuzLgth[7] == 1) begin
452	  if(ctRGB[21:8] == 0 || ctRGB[21:8] == 1 || ctRGB[21:8]  == 2) begin // idle @24bit~
453		  if(RGBreg[23-ctRGB[9:5]] == 0)	dptn <= 5'b01010;  	// "0" rgbout = 1'b1; 360/800
454		  else 								dptn <= 5'b10001;  	// "1" rgbout = 1'b1; 580/600
455		  if(ctRGB[4:0] < dptn[4:0]) rgbOUT <=  1; else rgbOUT <= 0;
456	  end else rgbOUT = 1'b0;     							// "0" reset  mode
457	 end
458  end
459 /***********************************************************************
460  * BUZZ controll BuzNote[7:0], BuzLgth[7:0]; // BUZ NOTE 0x04/0x96,97,98,99,9A,9B  noTon 0x0f
461 ***********************************************************************/
462  reg speaker;				// sound pulse
463  assign BUZZ = speaker;	// [5] A6
464  reg [27:0] tone;
465  reg [7:0] ntype; 		// 0x96,97,98,99,9A,9B/0x0f dis
466  always @(posedge clk) begin ntype <= BuzNote; end
467  always @(posedge clk) begin  tone <= tone+1; end
468  wire [6:0] fastsweep = (tone[22] ? tone[21:15] : ~tone[21:15]);
469  wire [6:0] slowsweep = (tone[25] ? tone[24:18] : ~tone[24:18]);
470  wire [14:0]  clkdivider = {2'b01, (tone[ntype[6:0]] ? slowsweep : fastsweep), 6'b000000}; 
471
472  reg [14:0] counter;
473  always @(posedge clk) if(counter==0) counter <= clkdivider;  else counter <= counter-1;
474  always @(posedge clk) if(counter==0 && ntype[7])  speaker <= ~speaker;
475	  
476endmodule
477

/* ------------------------------------------------------------------
   FPGA code
   MEDAMA PROJECT
   PORT/FPGApin  ASSIGNMENT
                     +-fpga-----+
                     |MACHXO2   8--->RGB(D7)
           (A7)IR--->11         5--->BUZ(A6)
                     |         20--->SV0
                     |         21--->SV1
                     |         14--->SV2(A3) can use for monitor
                     |         13--->SV3(A2) can use for monitor
    +-cam-------+    |          |    +-mpu----------+
    |OV767X     |    |   XCLK2 23<---D9  XCLK nano33|
    |           |    |      RX 25<---D12 TX   3.3V  |
    |        XCLK<---4  XCLK TX 26--->D11 RX         |
    |           |    |          |    |              |
    |        PCLK--->9 PCLK    10--->A0 DPCLK       |
    |       VSYNC--->16  VSYNC  17--->D8 DVSYNC  A4/A5
    I2C-BUS-HREF+    +---12-----+    +-A1--D0-7--I2C+
      |   |   +-----------+-HREF-------+     |    |
      |   +-----------------D0-D7-------//8--+    |
      +---------------------I2C-SDA[27]SCL[28]A5--+
   -----------------------------------------------------------------*/
module  TinyFPGA_A2 (       				
				input 	IR,			// pin11 ->IR{A7)
				output	TP14,		//  pin14 ->SV2(A3)
				output	TP13,		// pin13 ->SV3(A2)
		
				output	SV0,		//  pin20 ->
				output	SV1,		// pin21 ->
				output	BUZZ,		// pin5  ->BUZZ(A6)
				output	RGB,		// pin8  ->RGB(D7)

                input  	XCLK2,   	// pin23 <-D9 16MHz			
				output	XCLK,		// pin4  ->cam  15MHz
				input	PCLK,		// pin9  <-cam
				output	DPCLK,		//  pin10 ->A0 =PCLK
				input	VSYNC,		// pin16 <-cam
				output	DVSYNC,		//  pin17 ->D8 =VSYNC
				input	HREF,		// pin12(A1) <-cam
			
				input  	RX,       	// pin25 <-D12(TX@HOST)
				output	TX			// pin26 ->D11(RX@HOST)  TXTP26
					);
 /***********************************************************************
  * CAMERA logic                                                         *
 ***********************************************************************/
  //reg [2:0] HAFC;  always @(posedge clk) HAFC <= HAFC + 1;
  //assign XCLK  = HAFC[0];			//       (15MHz)     [4] -> XCLK 
  assign DPCLK = PCLK;				//  PCLK ------> [9] [10] -> DPCLK(A0) 
  assign DVSYNC = VSYNC;			// VSYNC ----->  [16][17] -> DVSYNC(D8)
									// HREF -(A1)-> [12]
									//  XCLK(library)---> D9
  //assign XCLK = XCLK2;			// 16MHz
  assign XCLK  = CLK;				// 30MHz
  //assign TP14 = HREF;	// PIN14  (A3)
  //assign  TP13 = VSYNC;	// PIN13  (A2)
 /***********************************************************************
  * SYSTEM CLOCK  29.56(33.8)/26.60(37.6)/24.18(41.3)/22.17(45.1)/20.46(48.9)
  ***********************************************************************/	  
  wire CLK;  // base clock 29.56MHz 33.25ns
  OSCH #(.NOM_FREQ("29.56")) internal_oscillator_inst  (.STDBY(1'b0), .OSC(CLK)); // 
  assign clk = CLK;  
  /***********************************************************************
  * IR receiver                  32bit from MSB to LSB
 --|_9ms_start_|-4ms-|_600us_|-("0")600us-|_600us_|-("1")1600us-|_600us_stop_|--  
 ***********************************************************************/
  wire IRn; reg IRnn;			// IR pin/glidge canceller
  assign IR = IRn;				//  pin11

  reg [7:0] IRcntL, IRcntH;	// input signal glidge canceller
  always  @(posedge clk) begin 	// remove glidge
	if(IRn == 0) begin IRcntL <= IRcntL  + 1; IRcntH <= 0;	end
	if(IRn == 1) begin	IRcntH <= IRcntH + 1; IRcntL <=  0;	end
	if(IRcntL > 20) IRnn <= 0;    if(IRcntH > 20) IRnn <= 1;
  end
  
  reg [31:0] IRreg; 			// 32bit receive data buffer
  reg [7:0] rep;				//  decoded 8bit char data (to uart)
  reg [5:0] irseq; 			// ir receive sequence  
  
  reg [19:0] ctirL, ctirH; 	// l/h clock(33.8ns) counter 
  reg ena,  pena;				// enable gate flags
  reg Ureq;						// uart request
  always @(posedge clk) begin	
	// enable signal from Startbit && H
	if(ctirH  > 20'h30000) begin pena <= 0; ena <= 1'b0; irseq <= 0; 	end // idle 6.4ms
	if(ctirL  > 20'h30000) begin pena <= 1;        					end // prenable
	if(pena == 1  && IRnn == 1) begin ena <= 1'b1;         			end	// enable
	if(Ureq == 1  && TxD_data == 0) Ureq <= 1'b0; // reset uart transfer request @TxD started
	//  receive 32 bit data
	if(ena == 1) 	begin
	if(IRnn == 0) 	begin	// "L"  ---- 
		if(ctirL == 0) begin //
			if(irseq > 0) begin
			if(ctirH  > 18'h4557) 	begin IRreg[32-irseq[5:0]] = 1'b1; end // > 600us 
			else                 	begin  IRreg[32-irseq[5:0]] = 1'b0; end // < 600us		
			if(irseq == 32) begin 		//		
				Ureq  <= 1'b1; 	   		// Uart request pulse on
				rep <= IRreg[7:0]; 	// LSB  8bit
  				//case(IRreg[15:0]) // decode data 32bit >> char 8bit
				//	16'hA25D:  rep <= 8'h31; // "1(0x31)" 
				//	16'h629D: rep <= 8'h32; // "2(0x32)"  
				//	16'hE21D: rep <= 8'h33; // "3(0x33)" 
				//	16'h22DD:  rep <= 8'h34; // "4(0x34)" 
				//	16'h02FD: rep <= 8'h35; // "5(0x35)"  
				//	16'hC23D: rep <= 8'h36; // "6(0x36)" 
				//	16'hE01F:  rep <= 8'h37; // "7(0x37)" 
				//	16'hA857: rep <= 8'h38; // "8(0x38)"
				//	16'h906F:  rep <= 8'h39; // "9(0x39)" 
				//	16'h9867: rep <= 8'h30; // "0(0x30)"  
				//	16'h6897: rep <= 8'h2A; // "*(0x2A)" 
				//	16'hB04F:  rep <= 8'h23; // "#(0x23)" 
				//	16'h18E7: rep <= 8'h55; // "U(0x55)"  
				//	16'h4AB5: rep <= 8'h44; // "D(0x44)" 
				//	16'h10EF:  rep <= 8'h4C; // "L(0x4C)" 
				//	16'h5AA5: rep <= 8'h52; // "R(0x52)"  
				//	16'h38C7: rep <= 8'h4B; // "K(0x4B)" *"C(0x43)":SVO COMP
				//endcase			
			end
			end
		end
	end  //"H" ----	 next step
	else if(ctirH == 0) irseq <= irseq + 1; 	
	end
	//  L/H pulse width counters
	if(IRnn == 0) 	begin				// "L" ---- 
		if(ctirL  != 0) ctirH <= 0; 	// first entry after H >> L(counter == 0)
		if(ctirL <  20'h45000) ctirL <= ctirL + 1; 	// "L" timer count up	9ms
	end else  		begin				//  "H" ----	
		if(ctirH != 0) ctirL <= 0; 	// first entry after L >> H(counter  == 0)
		if(ctirH < 20'h45000) ctirH <= ctirH + 1; 	// "H" timer count up
	end
  end
 /***********************************************************************
  * UART Transfer    output TX[25]--D12 RX@HOST  input RX[23]--D9 TX@HOST         
  [IR command to UART command transfer]                
    -->Receive IR data  v     reset by TxL
	                __X-[Ureq]-X_ v^
					   Start Uart  Tx transfer
 ***********************************************************************/	
  //reg [10:0] BaudDivCnt;				// [10:0] 11bit 14400 tick 69.6(67.8us)us
  reg [7:0] BaudDivCnt;				// [7:0] 8bit 115200 tick 8.7(8.5us)us
  wire  BaudTick = (BaudDivCnt == 15); 	// 

  reg [7:0] Ustate;			// state machine
  reg TxD_start, TxD_data;		// data line
  reg [7:0] Udata;				// Transfer  data rep[7:0];
  assign TX = TxD_data;		// pin26 to D11

  always @(posedge  clk) begin
  BaudDivCnt <= BaudDivCnt + 1;
  if(Ustate == 0) TxD_data  <=  0'b1;  // preset
  //- receive transfer request -------------------------
  if(Ureq && TxD_start == 0) 	begin //receive req/preset
	  TxD_start <= 1'b1;  Ustate <= 8'h00; Udata <= rep; 	// load transfer data (IR data/Trans comp)
  end
  if(SCreq && TxD_start == 0) 	begin //servo comp req/preset
	  TxD_start  <= 1'b1; Ustate <= 8'h00; Udata <= 8'h43; // "C" servo comp report
  end
  //- state machine-------------------------------------
  if(TxD_start && BaudTick)  Ustate <= Ustate + 1;
  case(Ustate)
	 8'h03: TxD_data <= 0'b0;		// Start  bit
	 8'h04: TxD_data <= Udata[0];	// out bit0
	 8'h05: TxD_data <= Udata[1];	//  out bit1
	 8'h06: TxD_data <= Udata[2];	// out bit2
	 8'h07: TxD_data <=  Udata[3];	// out bit3
	 8'h08: TxD_data <= Udata[4];	// out bit4
	 8'h09:  TxD_data <= Udata[5];	// out bit5
	 8'h0A: TxD_data <= Udata[6];	// out bit6
	  8'h0B: TxD_data <= Udata[7];	// out bit7
	 8'h0C: TxD_data  <= 0'b1;		//  out stopbit
	 8'h0D: TxD_data  <= 0'b1;		// out stopbit
	 8'h0E: TxD_start  <= 1'b0; 	// complete and close
  endcase
  end
 /***********************************************************************
  * UART Receive     output TX[25]--D12 RX@HOST  input RX[23]--D9 TX@HOST         
	ff  no action/00 reset counter
 ***********************************************************************/
  //reg [9:0] BaudDivCntR; 					// [9:0] 10bit 14400 double tick
  reg [6:0]  BaudDivCntR;					// [6:0] 7bit 115200 tick 8.7us/2
  wire BaudTickR = (BaudDivCntR  == 15); 	//

  reg RxD_start;
  wire RxD_data = RX; 			// loop back  test
  reg [5:0] Ubp;				// buffer pointer 8x5byte 0-40
  reg [6:0] UstateR;			//  state machine
  reg [39:0] URdtR;			// 5bytex8 data
  reg monit;					//  debug flag
  reg [3:0] CMD;				// command reg trasfer command to servo control
  
  always @(posedge clk) begin
  BaudDivCntR <= BaudDivCntR + 1;
  if(RxD_data  == 0 && RxD_start == 0) begin 	//receive request/preset
	   BaudDivCntR <=  14; // Sync with received RxD_data start
	   RxD_start <= 1'b1;  UstateR <=  8'h00; 	// need reset source
	   //monit <= 0;
  end 	
			
  if(RxD_start  && BaudTickR) UstateR <= UstateR + 1;
  case(UstateR)
	 8'd3:  if(BaudTickR)  begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit0
	 8'd5:  if(BaudTickR)  begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit1
	 8'd7:  if(BaudTickR)  begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit2	 
	 8'd9:  if(BaudTickR)  begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit3
	 8'd11: if(BaudTickR)  begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit4
	 8'd13: if(BaudTickR)  begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit5	 	 
	 8'd15: if(BaudTickR)  begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit6
	 8'd17: if(BaudTickR)  begin URdtR[Ubp] <= RxD_data; Ubp <= Ubp + 1; end // bit7
	 8'd19: if(BaudTickR)  begin	// stop bit, store to each registers
		if(Ubp == 8'd40) begin
			Ubp  <= 0; 				// Reset counter
			case(URdtR[7:0]) 		// byte0
				8'h01:  begin		// servo position command 0xff:do nothing
					CMD <= 8'h01;	//  set command received
					if(URdtR[15:8] != 8'hff)	chNSVO[0] <= URdtR[15:8];	//  SV0 target angle	 	
					if(URdtR[23:16] != 8'hff)	chNSVO[1] <= URdtR[23:16];	//  SV1 target angle
					if(URdtR[31:24] != 8'hff)	chNSVO[2] <= URdtR[31:24];	//  SV2 target angle	 	
					if(URdtR[39:32] != 8'hff)	chNSVO[3] <= URdtR[39:32];	//  SV3 target angle
					   end
				8'h02: begin		// servo speed command
					CMD  <= 8'h02;  // set command received
					if(URdtR[15:8] != 8'hff)  SVsped  <= URdtR[15:8];	 // wait counter for delay
					if(URdtR[23:16] != 8'hff)  SVstop <= URdtR[23:16]; // 0000xxxx target <= current
					else SVstop <=  0;									 // pause flag reset
					if(URdtR[31:24] != 8'hff)  SVswng <= URdtR[31:24]; // mode/depth(6)	
					   end
				8'h03:  begin 		// RGB led command
					RGBR[23:16] <= URdtR[23:16]-1;	// G
					RGBR[15:8]	<=  URdtR[15:8]-1; 		// R
					RGBR[7:0] 	<= URdtR[31:24]-1; 	// B
					   end
				8'h04: begin 		// BUZ command
					BuzNote <= URdtR[15:8];		//  0x96,97,98,99,9A,9B noTon 0x0f
					BuzLgth <= URdtR[23:16]; 		// Option  bit7:enable svo/led pulse
					   end
			endcase
		end
		else  begin // reset receive data buffer pointer
			if(		(Ubp == 8'd8) &&(URdtR[7:0]  	== 8'h00)	// closing @1st byte
				|| 	(Ubp == 8'd16)&&(URdtR[15:8]	==  8'h00)	// closing @2nd byte
				|| 	(Ubp == 8'd24)&&(URdtR[23:16]	== 8'h00)  	// closing @3rd byte
				|| 	(Ubp == 8'd32)&&(URdtR[31:24]	== 8'h00)  	// closing @4th byte
			) begin Ubp <= 0;  end // Reset counter
		end
	  end
	 8'd21: begin RxD_start <= 1'b0; CMD<= 0; end	// command received pulse  4us
  endcase
  end
  //--monitor-----------------------------------------
  //assign TP14 = svocpm[4];	// PIN14  (A3)
  //assign TP14 = monit;	// PIN14  (A3)
  //assign TP13 = RxD_data;		// PIN13  (A2)
  //--------------------------------------------------
  /***********************************************************************
 * Connection  REGISTER SET                      // xff:no load/0:no if  *
 ***********************************************************************/  
  reg [7:0] chNSVO[3:0];						// SERVO TARGET 0x01/byte1/byte2/byte3/byte4(1-0xfe)
  reg [7:0] SVsped, SVstop, SVswng; 	    	// SERVO SPEED 0x02/00010000(),wait/0001111(stp)
  reg [23:0] RGBR;								// RGB LED 0x03/red/green/blue()0x01-0xfe(-1)
  reg [7:0] BuzNote, BuzLgth;		 			// BUZ NOTE 0x04/0x96,97,98,99,9A,9B noTon  0x0f
 /***********************************************************************
  * SERVO controller     0.5ms---1.5ms---2.5ms                           *
      ____________  _________                                        ______
 ____|set         ^         |reset__________reset____//____reset___|set  ^ 
 con=0xAEFE  con=0xB4A2->0  con[11:2]==act[9:0] 0x0000-0x2d0(0-2ms) con=0xAEFE
	output	SV0,		//  pin20 ->
	output	SV1,		// pin21 ->
   	output	TP14,		// pin14 ->SV2(A3)
	output	TP13,		//  pin13 ->SV3(A2)
	BuzLgth[7] ebable pulse
	BuzLgth[6] ebable On track (stop  servo pulse/issue servo comp)
 ***********************************************************************/
  reg [3:0] svOUT;    	   						 	// output signalx4 lines
    assign  SV0  = svOUT[0];		// pin20
	assign SV1  = svOUT[1];		// pin21					
    assign TP14 = svOUT[2]; 		// pin14 SV2
    assign TP13 = svOUT[3]; 		//  pin13 SV3
  parameter CYCL = 20'h80000;						// 16ms Servo cycle
  parameter  PPAL = 16'h3f00;						// 0.5ms Pre Pulse    
  reg [19:0] svREG; 	     						//  Servo base counter
  
  reg [7:0] svTGT[3:0];							// 8bit target  register
  reg [9:0] svCRT[3:0];							// 10bit current count up
  reg  [9:0] svocpm; 								// set speed by uart
  reg onTrk;
  
  reg  [1:0] rot;								    // servo ch rotation
  always @(posedge clk) begin
	if(rot  == 0) rot <= 1; if(rot == 1) rot <= 2;
	if(rot == 2) rot <= 3; if(rot == 3)  rot <= 0;
  end
	
  always @(posedge clk) begin
	if(svREG > 17'h15000  && svREG < 19'h70000) begin // avoid servo positive pulse
	if(	{svTGT[0], 2'b00}  == svCRT[0] && {svTGT[1], 2'b00} == svCRT[1] &&
		{svTGT[2], 2'b00} == svCRT[2]  && {svTGT[3], 2'b00} == svCRT[3] && BuzLgth[6] == 1
	) onTrk <= 1; else onTrk  <= 0;
	end
  end
	
  always @(posedge clk) begin svREG <= svREG +  1; // timer count up
	if(BuzLgth[7] == 1) begin 
      if(onTrk == 0) begin
	  //-20bit-----4bit----10bit------6bit]------------------------
	  if(svREG  < {4'h0, svCRT[rot], 6'h0}) svOUT[rot] <= 1'b1; 			
	  else svOUT[rot] <=  1'b0; 						// PULSE GENERATE0	
	  if(svREG > CYCL - PPAL + 3000) svOUT  <= 4'hf;	// BASE 0.5ms PULSE				
	  if(svREG > CYCL) svREG <= 0; 					//  END CYCLE
	  end else svOUT <= 0;
	end
  end
 /***********************************************************************
  * us Event Control counter 29.56MHz(33.8ns)                            *
 ***********************************************************************/
  reg [14:0] cntU; reg uTick; parameter uDiv = 17750/3; 	// 29.56MHz(33.8ns) >>  2kHz(600us)  
  always @(posedge clk) uTick <= (cntU == uDiv-2);	   	// counter  reset pulse    
  always @(posedge clk) if(uTick) cntU <= 0; else cntU <= cntU  + 1; // |19.2|9.6|4.8|2.4|1.2|0.6ms|
 //---------------------------------------------------------------------
  reg [9:0] svo[3:0]; 						// wait control
  reg [9:0] svocp[3:0];						//  compare data delay

  always @(posedge clk) begin // INCREASE/DECREASE position  svCRT[rot]
	if(uTick) begin  // delay timer COUNT UP/DOWN
		svo[0] <= svo[0]  + 1'b1; svo[1] <= svo[1] + 1'b1;
		svo[2] <= svo[2] + 1'b1; svo[3] <= svo[3]  + 1'b1;
    end	
	if(svo[rot] > svocp[rot]) begin svo[rot] <= 10'h0;  		//  wait time
		if({svTGT[rot], 2'b00} > svCRT[rot]) svCRT[rot] <= svCRT[rot] +  1'b1; // increase
		if({svTGT[rot], 2'b00} < svCRT[rot]) svCRT[rot] <= svCRT[rot]  - 1'b1; // decrease			 
	end 
  end
  
  always @(posedge clk) begin  // SPEED UP by distance CONTROL  0---30(120)--60{240}--180(720) svocp[rot]
	if(svocpm[7]  == 0) begin // bit7:disable accel bit6-0:waiting value
	if({svTGT[rot], 2'b00}  > svCRT[rot]) begin 
		if({svTGT[rot], 2'b00} - svCRT[rot] > 240) 		svocp[rot]  <= {3'b00, svocpm[7:3]};
		else if({svTGT[rot], 2'b00} - svCRT[rot] < 100)  	svocp[rot] <= svocpm;
		else 												svocp[rot] <= {2'b00,  svocpm[7:2]}; 
	end // increase
	else if({svTGT[rot], 2'b00} < svCRT[rot])  begin 
		if(svCRT[rot] - {svTGT[rot], 2'b00} > 240) 		svocp[rot] <= {3'b00,  svocpm[7:3]};
		else if(svCRT[rot] - {svTGT[rot], 2'b00} < 100)	svocp[rot]  <= svocpm;
		else 												svocp[rot] <= {2'b00, svocpm[7:2]};  
	end // decrease 
	end else svocp[rot] <= svocpm[6:0]; // flat control
  end
/***********************************************************************
  * SERVO COMPLETE uart req                                             *
 ***********************************************************************/  
  reg SVCOMP;		// servo complete
  reg SCreq;		// servo comp report  request pulse
  reg [2:0] SMS; 	// state
  always @(posedge clk) begin SMS  <= SMS + 1;  
	if(swan[3:0] == 0) begin // SVswng[3:0] == 0 enable comp report
  	case(SMS)
		3'h01: begin
			if(onTrk) begin SVCOMP <= 1; if(SVCOMP  == 0) SCreq <= 1; end	// leading edge
			else SVCOMP <= 0; 
		end
		3'h06:  if(SCreq == 1) SCreq <= 0;
	endcase
	end
  end
 /***********************************************************************
  * ms Event Control LOAD COMMAND/BREATH move                            *
  svTGT[3:0],  svocpm	 // target/speed/stop bit INTERNAL
  chNSVO[3:0]			 // SERVO TARGET  0x01/byte1/byte2/byte3/byte4(1-0xfe)
  SVsped, SVstop, SVswng // SERVO SPEED  0x02/00010000(),wait/0001111(stp)
  [UART command to Servo execution]  
	-->Receive  UART command 5bytes
	                       X-[CMD]-Xv
                     load  to [catchCMD]v              ^ reset catch CMD 
					         load parameter  from command @mTicK
 ***********************************************************************/
  reg [14:0] cntM; reg mTick; parameter mDiv = 10000; 	// 400us 28000
  always  @(posedge clk) mTick <= (cntM == mDiv-2);		// counter reset pulse  
  always  @(posedge clk) if(mTick) cntM <= 0; else cntM <= cntM + 1'b1;  //
 //---------------------------------------------------------------------
  reg [11:0] led;  	// state machine
  reg [3:0]catchCMD;	// copy of uart command  reg CMD
  reg [7:0] swan;		// swing parameters polarity4bit + depth 4bit
  always @(posedge clk) begin
	  if(mTick && RxD_start == 0) begin led <= led  + 1'b1; 
	    case(led) // 1sec interval counter sequence control
		  default:  begin // load uart command every 400us
			      // access svTGT, sync with  CMD/catchCMD ontime after receiving uart command
				  // need 1ms interval  after command 0x01/0x02
				  if(catchCMD == 2'h01) begin 	// angle01/2/3
					  svTGT[0] <=  chNSVO[0];	// ch0 load target angle from uart
					 svTGT[1]  <= ~chNSVO[1];	// ch1
					 svTGT[2] <=  chNSVO[2];	// ch2
					  svTGT[3] <= ~chNSVO[3];	// ch3
					 catchCMD <= 0;  			// reset catched  command (ack)
					 monit <= 0;
				  end
				  if(catchCMD  == 2'h02) begin 	// speed/stop/swing
					 if(SVstop[0] == 1) svTGT[0][7:0]  <= svCRT[0][9:2]; 	// ch0
					 if(SVstop[1] == 1) svTGT[1][7:0] <= svCRT[1][9:2];  	// ch1
					 if(SVstop[2] == 1) svTGT[2][7:0] <= svCRT[2][9:2]; 	// ch2
					  if(SVstop[3] == 1) svTGT[3][7:0] <= svCRT[3][9:2]; 	// ch3
					 swan <=  SVswng;			//
					 //svocpm <= 8'b00010000; 	// set data from uart
					  svocpm <= SVsped;			// load speed reg from uart
				     catchCMD <= 0;  			// reset catched command (ack)
					 monit <= 1;
				  end
			    end
		  {4'b0100, 8'h00}: begin 
			    if(swan[3:0] != 0) begin
				  if(swan[4] == 0 && svTGT[0] > 32) svTGT[0] = svTGT[0] - {swan[3:0], 1'b0}; //front
				  else if(svTGT[0]< 223) svTGT[0] = svTGT[0] + {swan[3:0], 1'b0};
				  if(swan[5]  == 0 && svTGT[1] > 32) svTGT[1] = svTGT[1] - {swan[3:0], 1'b0}; 
				  else  if(svTGT[1]< 223) svTGT[1] = svTGT[1] + {SVswng[3:0], 1'b0};
			//	  if(swan[6]  == 0 && svTGT[2] > 32) svTGT[2] = svTGT[2] - {swan[3:0], 1'b0}; // rear
			//	  else if(svTGT[2]< 223) svTGT[2] = svTGT[2] + {SVswng[3:0], 1'b0}; 
			//	  if(swan[7] == 0 && svTGT[3] > 32) svTGT[3] = svTGT[3] - {swan[3:0], 1'b0}; 
			//	  else if(svTGT[3]< 223) svTGT[3] = svTGT[3] + {swan[3:0], 1'b0}; 
				end
				end  								//
		  {4'b1100, 8'h00}: begin 
			    if(swan[3:0] !=  0) begin
				  if(swan[4] != 0 && svTGT[0] > 32) svTGT[0] = svTGT[0] - {swan[3:0],  1'b0}; // front
				  else if(svTGT[0]< 223) svTGT[0] = svTGT[0] + {swan[3:0],  1'b0};
				  if(swan[5] != 0 && svTGT[1] > 32) svTGT[1] = svTGT[1] - {swan[3:0],  1'b0}; 
				  else if(svTGT[1]< 223) svTGT[1] = svTGT[1] + {swan[3:0], 1'b0};
			//	  if(swan[6] != 0 && svTGT[2] > 32) svTGT[2] = svTGT[2] - {swan[3:0], 1'b0}; //  rear
			//	  else if(svTGT[2]< 223) svTGT[2] = svTGT[2] + {swan[3:0], 1'b0};
			//	  if(swan[7] != 0 && svTGT[3] > 32) svTGT[3] = svTGT[3] - {swan[3:0], 1'b0}; 
			//	  else if(svTGT[3]< 223) svTGT[3] = svTGT[3] + {swan[3:0], 1'b0};
				end
				end  
	    endcase
	  end else begin if(CMD != 0) begin catchCMD <= CMD; end  // catch 4us 0x01/0x02 command pulse
	  end // catch uart command
  end
  /***********************************************************************
 * RGB  LED controller  G-R-B 24bit [11:8](RGB+5byte blank) [7:5](byte cntr) [4:0](0(10/22)
  _|-2-500ns-|__650--950ns_____|"0" _|--550--650ns-|__450--750ns_|"1" -|_//_>50us_|-  "reset"  
  BuzLgth[7] ebable pulse 
 ***********************************************************************/
  reg rgbOUT;    	    // output signal
  assign RGB = rgbOUT;	// [8] D7
  reg [23:0] RGBreg;	// 3byte RGB color set through uart
  reg [21:0] ctRGB;	//  rgb clock(33.25ns) 21-8(byte pointer)/9-5(24bit pointer)
  reg [4:0] dptn; 		//  data pattern "0/1" compare reg
  always @(posedge clk) begin ctRGB <= ctRGB  + 1;
	  RGBreg <= RGBR;	// RGBR[23:0]: LED 0x03/red/green/blue()0x01-0xfe(-1)
	  if(BuzLgth[7] == 1) begin
	  if(ctRGB[21:8] == 0 || ctRGB[21:8] == 1 || ctRGB[21:8]  == 2) begin // idle @24bit~
		  if(RGBreg[23-ctRGB[9:5]] == 0)	dptn <= 5'b01010;  	// "0" rgbout = 1'b1; 360/800
		  else 								dptn <= 5'b10001;  	// "1" rgbout = 1'b1; 580/600
		  if(ctRGB[4:0] < dptn[4:0]) rgbOUT <=  1; else rgbOUT <= 0;
	  end else rgbOUT = 1'b0;     							// "0" reset  mode
	 end
  end
 /***********************************************************************
  * BUZZ controll BuzNote[7:0], BuzLgth[7:0]; // BUZ NOTE 0x04/0x96,97,98,99,9A,9B  noTon 0x0f
 ***********************************************************************/
  reg speaker;				// sound pulse
  assign BUZZ = speaker;	// [5] A6
  reg [27:0] tone;
  reg [7:0] ntype; 		// 0x96,97,98,99,9A,9B/0x0f dis
  always @(posedge clk) begin ntype <= BuzNote; end
  always @(posedge clk) begin  tone <= tone+1; end
  wire [6:0] fastsweep = (tone[22] ? tone[21:15] : ~tone[21:15]);
  wire [6:0] slowsweep = (tone[25] ? tone[24:18] : ~tone[24:18]);
  wire [14:0]  clkdivider = {2'b01, (tone[ntype[6:0]] ? slowsweep : fastsweep), 6'b000000}; 

  reg [14:0] counter;
  always @(posedge clk) if(counter==0) counter <= clkdivider;  else counter <= counter-1;
  always @(posedge clk) if(counter==0 && ntype[7])  speaker <= ~speaker;
	  
endmodule

Downloadable files

Schematic Drawing

Documentation

3D print data

Face Front, Face Back, Base Stand, Face Holder, Arm, Battery Holder, Camera Spacer

3D print data

Face Front, Face Back, Base Stand, Face Holder, Arm, Battery Holder, Camera Spacer

3D print data

Gerber data

Comments

Only logged in users can leave comments

ebaera

0 Followers

•

0 Projects

Intro