0001:	L_SPEED_PID:	mov	bl,byte ptr ds:[MOTOR_DIR]
0002:			mov	dx,word ptr ds:[ENC1_DATA]
0003:			in	ax,ENC1
0004:			mov	word ptr ds:[ENC1_DATA],ax
0005:			and	bl,01000000b
0006:			jnz	lpid0010
0007:			xchg	ax,dx
0008:	lpid0010:	sub	ax,dx
0009:			mov	word ptr ds:[L_SPEED],ax
0010:			mov	dx,word ptr ds:[L_SPEED0]
0011:			sub	dx,ax
0012:			mov	ax,word ptr ds:[L_SPEED_ERR1]	;load En-1
0013:			mov	word ptr ds:[L_SPEED_ERR2],ax	;save En-1
0014:			mov	word ptr ds:[L_SPEED_ERR1],dx	;save En
0015:			mov	bx,word ptr ds:[L_SPEED_ERR3]	;load En-1+En-2 ...
0016:			add	bx,dx
0017:			mov	word ptr ds:[L_SPEED_ERR3],bx	;save En+En-1 ...
0018:	
0019:			mov	ax,word ptr ds:[L_SPEED_ERR1]
0020:			mov	cx,30				;Kp(x50)
0021:			imul	cx				;Kp x En
0022:			push	ax
0023:			mov	ax,word ptr ds:[L_SPEED_ERR3]
0024:			mov	cx,10				;Ki(x50)
0025:			imul	cx				;Ki x (En + En-1 ...)
0026:			push	ax
0027:			mov	ax,word ptr ds:[L_SPEED_ERR1]
0028:			sub	ax,word ptr ds:[L_SPEED_ERR2]
0029:			mov	cx,2				;Kd(x50)
0030:			imul	cx				;Kd x (En-En-1)
0031:			pop	dx
0032:			add	ax,dx
0033:			pop	dx
0034:			add	ax,dx
0035:			mov	cx,50
0036:			mov	dx,0
0037:			test	ah,10000000b
0038:			jz	lpid0020
0039:			not	dx
0040:	lpid0020:	idiv	cx
0041:			mov	dl,byte ptr ds:[MOTOR_DIR]
0042:			test	ah,10000000b
0043:			jz	lpid0030
0044:			neg	ax
0045:			mov	cl,6
0046:			db	00001111b,00010110b,11000010b	;NOT1	dl,cl
0047:	lpid0030:	mov	bx,word ptr ds:[L_SPEED]	;torque limit
0048:			test	bh,10000000b
0049:			jz	lpid0040
0050:			neg	bx
0051:	lpid0040:	sar	bx,2				;1/4
0052:			add	bx,20
0053:			cmp	ax,bx
0054:			jc	lpid0050
0055:			mov	ax,bx
0056:	lpid0050:	mov	byte ptr ds:[L_PWM_DATA],al
0057:			out	PWM0,al
0058:			in	al,PWMREG
0059:			and	al,10111111b
0060:			or	al,dl
0061:			out	PWMREG,al			;set motor-dir