Con Sys_CLK = 80_000_000 BAUD_RATE = 3000000 Rx_PIN = 63 Tx_PIN = 62 servofeedback_pin = 32 'servo feedback pin nco = (sys_clk / baud_rate) * $1_0000 & $FFFFFC00 nco_f = (sys_clk - ((sys_clk / baud_rate) * baud_rate)) * 64 / baud_rate bitper = nco | nco_f << 10 |7 _PWM = 2920 '******************* dat org clkset #$FF coginit #1, ##@serialplus_servo_feedback coginit #2, ##@drive_servo main jmp #main '********* dat org serialplus_servo_feedback wrpin pm_high,#servofeedback_pin dirh #servofeedback_pin WRPIN tx_mode, #Tx_PIN 'setup serial wxpin ##bitper,#tx_pin wrpin rx_mode, #Rx_Pin 'set asynchronous rx mode in smart pin wxpin ##bitper,#Rx_pin dirh #Tx_Pin 'start smart pin dirh #Rx_Pin 'start smart pin waitx ##320_000_000 'time to manually enable Parallax Serial Terminal mov tx_char,#13 call #send_first 'prime the serial process call #send1_char mov previous_state,#0 get_servo_position testp #servofeedback_pin wc if_nc jmp #get_servo_position RdPIN hperiod,#servofeedback_pin mov temp3,hperiod add hperiod,previous_state mov previous_state,temp3 mov value,hperiod 'which number to send mov tx_char,#13 call #send1_char loc ptrb,#@hstring 'etc, etc call #send_string call #send_dec 'uses "value" jmp #get_servo_position '************************************* ser_loop testp #Rx_Pin wc 'wait for smart pin to signal rx data received if_nc ret call #rcv1_char mov Tx_char,Rx_char call #send1_char jmp #ser_loop '********************* send_string 'thanks ozpropdev 'loc ptrb,#@tstring bbb rdbyte tx_char,ptrb++ wz if_z ret call #send1_char jmp #bbb '************************************** send_dec cmps value,#0 wc if_c mov tx_char,#"-" if_c neg value,value if_c call #send1_char mov i,##1_000_000_000 mov temp,#10 .decLoop qdiv value,i waitx #60 getqx TX_char getqy value add TX_char,#48 call #send1_char qdiv i,#10 waitx #60 getqx i djnz temp, #.decloop ret '*************************************** rcv1_char testp #Rx_Pin wc 'wait for smart pin to signal rx data received if_nc jmp #rcv1_char rcv1_char1 RDPIN rx_char,#Rx_Pin shr rx_char,#24 ret '****************** send1_char testp #Tx_pin WC if_nc jmp #send1_char send_first WYPIN Tx_char,#Tx_Pin ret '******************* _neg_edge long %0_10_000000 _pos_Edge long %0_01_000000 tx_mode long %0000_0000_000_0000000000000_01_11110_0 'async tx mode, rx_mode long %0000_0000_000_0000000000000_01_11111_0 'async rx mode, pm_high long %0000_0000_000_0000000000000_00_10000_0 'time A high states 'bitper long (sys_clk/BAUD_RATE)*$10_000 & $FFFFFC00 | 7 'sys_clk/baud_rate + bits-1 tx_char res 1 rx_char res 1 tx_long res 1 temp res 1 temp2 res 1 temp3 res 1 test_char res 1 value res 1 lvalue res 1 i res 1 _edg res 1 t1 res 1 t2 res 1 t3 res 1 hperiod res 1 lperiod res 1 period res 1 samples res 1 previous_state res 1 '************************ Dat orgh $400 hstring byte " high: ",0 lstring byte " low: ",0 periodstring byte " total period h+l: ",0 sample byte "#",0 '************************ '*********************************************************************************** dat org drive_servo mov pwm,##_pwm mov x,#2 'x is smart pin wrpin pm,x 'set triangle pwm mode in smart pin #0 wxpin fr,x 'set frame count and prescaler wypin pwm,x dirh x 'start smart pin pm long %1_01000_0 'triangle pwm mode fr long 40000<<16 + 20 'frame is 40000 counts, prescaler is 20 = 50Hz (20ms) 'this gives pwm resolution of 0.02/40000= 0.0000005s =0.5us pwm res 1 'on for 100/40000 of pwm period x res 1 reps res 1