mousealgorithm.s

陀螺仪程序

	mov [__r0],[_gyroY]
	mov [__r1],[_gyroY+1]
	mov [__r2],[_gyroY+2]
	mov [__r3],[_gyroY+3]
	mov A,2
X47:
	asr [__r0]
	rrc [__r1]
	rrc [__r2]
	rrc [__r3]
	dec A
	jnz X47
	mov A,[__r3]
	sub [_gyroY+3],A
	mov A,[__r2]
	sbb [_gyroY+2],A
	mov A,[__r1]
	sbb [_gyroY+1],A
	mov A,[__r0]
	sbb [_gyroY],A
	.dbline 299
;         gyroZ=gyroZ-(gyroZ>>2);
	mov [__r0],[_gyroZ]
	mov [__r1],[_gyroZ+1]
	mov [__r2],[_gyroZ+2]
	mov [__r3],[_gyroZ+3]
	mov A,2
X48:
	asr [__r0]
	rrc [__r1]
	rrc [__r2]
	rrc [__r3]
	dec A
	jnz X48
	mov A,[__r3]
	sub [_gyroZ+3],A
	mov A,[__r2]
	sbb [_gyroZ+2],A
	mov A,[__r1]
	sbb [_gyroZ+1],A
	mov A,[__r0]
	sbb [_gyroZ],A
	.dbline 300
; 	}
L95:
	.dbline 301
; 	if (mode==1) {
	cmp [_mode],1
	jnz L97
	.dbline 301
	.dbline 307
; 
; 		#ifdef DEBUG_INFO
; 		printf("SPEED 1\n");
; 		#endif
; 
;         gyroY=gyroY-(gyroY>>2)-(gyroY>>3);
	mov [__r0],[_gyroY]
	mov [__r1],[_gyroY+1]
	mov [__r2],[_gyroY+2]
	mov [__r3],[_gyroY+3]
	mov A,3
X49:
	asr [__r0]
	rrc [__r1]
	rrc [__r2]
	rrc [__r3]
	dec A
	jnz X49
	mov [__r4],[_gyroY]
	mov [__r5],[_gyroY+1]
	mov [__r6],[_gyroY+2]
	mov [__r7],[_gyroY+3]
	mov A,2
X50:
	asr [__r4]
	rrc [__r5]
	rrc [__r6]
	rrc [__r7]
	dec A
	jnz X50
	mov A,[_gyroY+3]
	sub A,[__r7]
	mov [__r7],A
	mov A,[_gyroY+2]
	sbb A,[__r6]
	mov [__r6],A
	mov A,[_gyroY+1]
	sbb A,[__r5]
	mov [__r5],A
	mov A,[_gyroY]
	sbb A,[__r4]
	mov [__r4],A
	mov A,[__r7]
	sub A,[__r3]
	mov [_gyroY+3],A
	mov A,[__r6]
	sbb A,[__r2]
	mov [_gyroY+2],A
	mov A,[__r5]
	sbb A,[__r1]
	mov [_gyroY+1],A
	mov A,[__r4]
	sbb A,[__r0]
	mov [_gyroY],A
	.dbline 308
;         gyroZ=gyroZ-(gyroZ>>2)-(gyroZ>>3);
	mov [__r0],[_gyroZ]
	mov [__r1],[_gyroZ+1]
	mov [__r2],[_gyroZ+2]
	mov [__r3],[_gyroZ+3]
	mov A,3
X51:
	asr [__r0]
	rrc [__r1]
	rrc [__r2]
	rrc [__r3]
	dec A
	jnz X51
	mov [__r4],[_gyroZ]
	mov [__r5],[_gyroZ+1]
	mov [__r6],[_gyroZ+2]
	mov [__r7],[_gyroZ+3]
	mov A,2
X52:
	asr [__r4]
	rrc [__r5]
	rrc [__r6]
	rrc [__r7]
	dec A
	jnz X52
	mov A,[_gyroZ+3]
	sub A,[__r7]
	mov [__r7],A
	mov A,[_gyroZ+2]
	sbb A,[__r6]
	mov [__r6],A
	mov A,[_gyroZ+1]
	sbb A,[__r5]
	mov [__r5],A
	mov A,[_gyroZ]
	sbb A,[__r4]
	mov [__r4],A
	mov A,[__r7]
	sub A,[__r3]
	mov [_gyroZ+3],A
	mov A,[__r6]
	sbb A,[__r2]
	mov [_gyroZ+2],A
	mov A,[__r5]
	sbb A,[__r1]
	mov [_gyroZ+1],A
	mov A,[__r4]
	sbb A,[__r0]
	mov [_gyroZ],A
	.dbline 309
; 	}
L97:
	.dbline 310
; 	if (mode==2) {
	cmp [_mode],2
	jnz L99
	.dbline 310
	.dbline 316
; 
; 		#ifdef DEBUG_INFO
; 		printf("SPEED 2\n");
; 		#endif
; 
;         gyroY=(gyroY>>1);
	asr [_gyroY]
	rrc [_gyroY+1]
	rrc [_gyroY+2]
	rrc [_gyroY+3]
	.dbline 317
;         gyroZ=(gyroZ>>1);
	asr [_gyroZ]
	rrc [_gyroZ+1]
	rrc [_gyroZ+2]
	rrc [_gyroZ+3]
	.dbline 318
; 	}
L99:
	.dbline 319
; 	if (mode==3) {
	cmp [_mode],3
	jnz L101
	.dbline 319
	.dbline 325
; 
; 		#ifdef DEBUG_INFO
; 		printf("SPEED 3\n");
; 		#endif
; 
;         gyroY=(gyroY>>1)-(gyroY>>3);
	mov [__r0],[_gyroY]
	mov [__r1],[_gyroY+1]
	mov [__r2],[_gyroY+2]
	mov [__r3],[_gyroY+3]
	mov A,3
X53:
	asr [__r0]
	rrc [__r1]
	rrc [__r2]
	rrc [__r3]
	dec A
	jnz X53
	mov [__r4],[_gyroY]
	mov [__r5],[_gyroY+1]
	mov [__r6],[_gyroY+2]
	mov [__r7],[_gyroY+3]
	asr [__r4]
	rrc [__r5]
	rrc [__r6]
	rrc [__r7]
	mov A,[__r7]
	sub A,[__r3]
	mov [_gyroY+3],A
	mov A,[__r6]
	sbb A,[__r2]
	mov [_gyroY+2],A
	mov A,[__r5]
	sbb A,[__r1]
	mov [_gyroY+1],A
	mov A,[__r4]
	sbb A,[__r0]
	mov [_gyroY],A
	.dbline 326
;         gyroZ=(gyroZ>>1)-(gyroZ>>3);
	mov [__r0],[_gyroZ]
	mov [__r1],[_gyroZ+1]
	mov [__r2],[_gyroZ+2]
	mov [__r3],[_gyroZ+3]
	mov A,3
X54:
	asr [__r0]
	rrc [__r1]
	rrc [__r2]
	rrc [__r3]
	dec A
	jnz X54
	mov [__r4],[_gyroZ]
	mov [__r5],[_gyroZ+1]
	mov [__r6],[_gyroZ+2]
	mov [__r7],[_gyroZ+3]
	asr [__r4]
	rrc [__r5]
	rrc [__r6]
	rrc [__r7]
	mov A,[__r7]
	sub A,[__r3]
	mov [_gyroZ+3],A
	mov A,[__r6]
	sbb A,[__r2]
	mov [_gyroZ+2],A
	mov A,[__r5]
	sbb A,[__r1]
	mov [_gyroZ+1],A
	mov A,[__r4]
	sbb A,[__r0]
	mov [_gyroZ],A
	.dbline 327
; 	}
L101:
	.dbline 328
; 	if (mode==4) {
	cmp [_mode],4
	jnz L103
	.dbline 328
	.dbline 334
; 
; 		#ifdef DEBUG_INFO
; 		printf("SPEED 4\n");
; 		#endif
; 
;         gyroY=(gyroY>>1)-(gyroY>>2);
	mov [__r0],[_gyroY]
	mov [__r1],[_gyroY+1]
	mov [__r2],[_gyroY+2]
	mov [__r3],[_gyroY+3]
	mov A,2
X55:
	asr [__r0]
	rrc [__r1]
	rrc [__r2]
	rrc [__r3]
	dec A
	jnz X55
	mov [__r4],[_gyroY]
	mov [__r5],[_gyroY+1]
	mov [__r6],[_gyroY+2]
	mov [__r7],[_gyroY+3]
	asr [__r4]
	rrc [__r5]
	rrc [__r6]
	rrc [__r7]
	mov A,[__r7]
	sub A,[__r3]
	mov [_gyroY+3],A
	mov A,[__r6]
	sbb A,[__r2]
	mov [_gyroY+2],A
	mov A,[__r5]
	sbb A,[__r1]
	mov [_gyroY+1],A
	mov A,[__r4]
	sbb A,[__r0]
	mov [_gyroY],A
	.dbline 335
;         gyroZ=(gyroZ>>1)-(gyroZ>>2);
	mov [__r0],[_gyroZ]
	mov [__r1],[_gyroZ+1]
	mov [__r2],[_gyroZ+2]
	mov [__r3],[_gyroZ+3]
	mov A,2
X56:
	asr [__r0]
	rrc [__r1]
	rrc [__r2]
	rrc [__r3]
	dec A
	jnz X56
	mov [__r4],[_gyroZ]
	mov [__r5],[_gyroZ+1]
	mov [__r6],[_gyroZ+2]
	mov [__r7],[_gyroZ+3]
	asr [__r4]
	rrc [__r5]
	rrc [__r6]
	rrc [__r7]
	mov A,[__r7]
	sub A,[__r3]
	mov [_gyroZ+3],A
	mov A,[__r6]
	sbb A,[__r2]
	mov [_gyroZ+2],A
	mov A,[__r5]
	sbb A,[__r1]
	mov [_gyroZ+1],A
	mov A,[__r4]
	sbb A,[__r0]
	mov [_gyroZ],A
	.dbline 336
; 	}
L103:
	.dbline 337
; 	if (mode==5) {
	cmp [_mode],5
	jnz L105
	.dbline 337
	.dbline 343
; 
; 		#ifdef DEBUG_INFO
; 		printf("SPEED 5\n");
; 		#endif
; 
;         gyroY=(gyroY>>2);
	mov [__r0],[_gyroY]
	mov [__r1],[_gyroY+1]
	mov [__r2],[_gyroY+2]
	mov [__r3],[_gyroY+3]
	mov A,2
X57:
	asr [__r0]
	rrc [__r1]
	rrc [__r2]
	rrc [__r3]
	dec A
	jnz X57
	mov [_gyroY],[__r0]
	mov [_gyroY+1],[__r1]
	mov [_gyroY+2],[__r2]
	mov [_gyroY+3],[__r3]
	.dbline 344
;         gyroZ=(gyroZ>>2);
	mov [__r0],[_gyroZ]
	mov [__r1],[_gyroZ+1]
	mov [__r2],[_gyroZ+2]
	mov [__r3],[_gyroZ+3]
	mov A,2
X58:
	asr [__r0]
	rrc [__r1]
	rrc [__r2]
	rrc [__r3]
	dec A
	jnz X58
	mov [_gyroZ],[__r0]
	mov [_gyroZ+1],[__r1]
	mov [_gyroZ+2],[__r2]
	mov [_gyroZ+3],[__r3]
	.dbline 345
; 	}
L105:
	.dbline 346
; 	if (mode==6) {
	cmp [_mode],6
	jnz L107
	.dbline 346
	.dbline 352
; 
; 		#ifdef DEBUG_INFO
; 		printf("SPEED 6\n");
; 		#endif
; 
;         gyroY=gyroY>>3;
	mov [__r0],[_gyroY]
	mov [__r1],[_gyroY+1]
	mov [__r2],[_gyroY+2]
	mov [__r3],[_gyroY+3]
	mov A,3
X59:
	asr [__r0]
	rrc [__r1]
	rrc [__r2]
	rrc [__r3]
	dec A
	jnz X59
	mov [_gyroY],[__r0]
	mov [_gyroY+1],[__r1]
	mov [_gyroY+2],[__r2]
	mov [_gyroY+3],[__r3]
	.dbline 353
;         gyroZ=gyroZ>>3;
	mov [__r0],[_gyroZ]
	mov [__r1],[_gyroZ+1]
	mov [__r2],[_gyroZ+2]
	mov [__r3],[_gyroZ+3]
	mov A,3
X60:
	asr [__r0]
	rrc [__r1]
	rrc [__r2]
	rrc [__r3]
	dec A
	jnz X60
	mov [_gyroZ],[__r0]
	mov [_gyroZ+1],[__r1]
	mov [_gyroZ+2],[__r2]
	mov [_gyroZ+3],[__r3]
	.dbline 354
; 	}
L107:
	.dbline 356
; 	//"STOP MODE", when gyro pointer is staying within a small area
; 	if (mode==7) {
	cmp [_mode],7
	jnz L109
	.dbline 356
	.dbline 362
; 
; 		#ifdef DEBUG_INFO
; 		printf("STOP\n");
; 		#endif
; 
; 		gyroY=gyroY>>5;
	mov [__r0],[_gyroY]
	mov [__r1],[_gyroY+1]
	mov [__r2],[_gyroY+2]
	mov [__r3],[_gyroY+3]
	mov A,5
X61:
	asr [__r0]
	rrc [__r1]
	rrc [__r2]
	rrc [__r3]
	dec A
	jnz X61
	mov [_gyroY],[__r0]
	mov [_gyroY+1],[__r1]
	mov [_gyroY+2],[__r2]
	mov [_gyroY+3],[__r3]
	.dbline 363
; 		gyroZ=gyroZ>>5;
	mov [__r0],[_gyroZ]
	mov [__r1],[_gyroZ+1]
	mov [__r2],[_gyroZ+2]
	mov [__r3],[_gyroZ+3]
	mov A,5
X62:
	asr [__r0]
	rrc [__r1]
	rrc [__r2]
	rrc [__r3]
	dec A
	jnz X62
	mov [_gyroZ],[__r0]
	mov [_gyroZ+1],[__r1]
	mov [_gyroZ+2],[__r2]
	mov [_gyroZ+3],[__r3]
	.dbline 364
; 		if (gyroY>25) {gyroY=25;}
	mov A,25
	sub A,[_gyroY+3]
	mov A,0
	sbb A,[_gyroY+2]
	mov A,0
	sbb A,[_gyroY+1]
	mov A,[_gyroY]
	xor A,-128
	mov [__rX],A
	mov A,(0 ^ 0x80)
	sbb A,[__rX]
	jnc L111
X63:
	.dbline 364
	.dbline 364
	mov [_gyroY],0
	mov [_gyroY+1],0
	mov [_gyroY+2],0
	mov [_gyroY+3],25
	.dbline 364
L111:
	.dbline 365
; 		if (gyroY<-25) {gyroY=-25;}
	mov A,[_gyroY+3]
	sub A,-25
	mov A,[_gyroY+2]
	sbb A,-1
	mov A,[_gyroY+1]
	sbb A,-1
	mov A,[_gyroY]
	xor A,-128
	sbb A,(255 ^ 0x80)
	jnc L113
X64:
	.dbline 365
	.dbline 365
	mov [_gyroY],-1
	mov [_gyroY+1],-1
	mov [_gyroY+2],-1
	mov [_gyroY+3],-25
	.dbline 365
L113:
	.dbline 366
; 		if (gyroZ>25) {gyroZ=25;}
	mov A,25
	sub A,[_gyroZ+3]
	mov A,0
	sbb A,[_gyroZ+2]
	mov A,0
	sbb A,[_gyroZ+1]
	mov A,[_gyroZ]
	xor A,-128
	mov [__rX],A
	mov A,(0 ^ 0x80)
	sbb A,[__rX]
	jnc L115
X65:
	.dbline 366
	.dbline 366
	mov [_gyroZ],0
	mov [_gyroZ+1],0
	mov [_gyroZ+2],0
	mov [_gyroZ+3],25
	.dbline 366
L115:
	.dbline 367
; 		if (gyroZ<-25) {gyroZ=-25;}
	mov A,[_gyroZ+3]
	sub A,-25
	mov A,[_gyroZ+2]
	sbb A,-1
	mov A,[_gyroZ+1]
	sbb A,-1
	mov A,[_gyroZ]
	xor A,-128
	sbb A,(255 ^ 0x80)
	jnc L117
X66:
	.dbline 367
	.dbline 367
	mov [_gyroZ],-1
	mov [_gyroZ+1],-1
	mov [_gyroZ+2],-1
	mov [_gyroZ+3],-25
	.dbline 367
L117:
	.dbline 368
; 	}
L109:
	.dbline 370
; 	//"OFF" Mode, when the gyro is on a desk, and only sensor noise is detected
; 	if (mode==8) {
	cmp [_mode],8
	jnz L119
	.dbline 370
	.dbline 376
; 
; 		#ifdef DEBUG_INFO
; 		printf("OFF\n");
; 		#endif
; 
; 		gyroY=0;	
	mov [_gyroY],0
	mov [_gyroY+1],0
	mov [_gyroY+2],0
	mov [_gyroY+3],0
	.dbline 377
; 		gyroZ=0;
	mov [_gyroZ],0
	mov [_gyroZ+1],0
	mov [_gyroZ+2],0
	mov [_gyroZ+3],0
	.dbline 378
; 	}
L119:
	.dbline 380
; 	//When button is detected, temporarily freeze the gyro data
; 	if (buttonCounter<25) {
	cmp [_buttonCounter],25
	jnc L121
X67:
	.dbline 380
	.dbline 386
; 
; 		#ifdef DEBUG_INFO
; 		printf("CLICK DETECTED\n");
; 		#endif
; 
; 		buttonCounter+=1;
	inc [_buttonCounter]
	.dbline 387
; 		gyroY = 0;
	mov [_gyroY],0
	mov [_gyroY+1],0
	mov [_gyroY+2],0
	mov [_gyroY+3],0
	.dbline 388
; 		gyroZ = 0;		
	mov [_gyroZ],0
	mov [_gyroZ+1],0
	mov [_gyroZ+2],0
	mov [_gyroZ+3],0
	.dbline 389
; 	}
L121:
	.dbline 390
; 	if ((*button!=lastButton)){
	mov A,[X-14]
	mov [__r1],A
	mvi A,[__r1]
	cmp A,[_lastButton]
	jz L123
	.dbline 390
	.dbline 391
; 		buttonCounter=0;
	mov [_buttonCounter],0
	.dbline 392
; 	}
L123:
	.dbline 393
	mov A,[X-14]
	mov [__r1],A
	mvi A,[__r1]
	mov [_lastButton],A
	.dbline 396
	mov A,[_gyroY+3]
	add [_gyroYInt+3],A
	mov A,[_gyroY+2]
	adc [_gyroYInt+2],A
	mov A,[_gyroY+1]
	adc [_gyroYInt+1],A
	mov A,[_gyroY]
	adc [_gyroYInt],A
	.dbline 397
	mov A,[_gyroZ+3]
	sub [_gyroZInt+3],A
	mov A,[_gyroZ+2]
	sbb [_gyroZInt+2],A
	mov A,[_gyroZ+1]
	sbb [_gyroZInt+1],A
	mov A,[_gyroZ]
	sbb [_gyroZInt],A
	.dbline 400
	mov [__r0],[_gyroYInt]
	mov [__r1],[_gyroYInt+1]
	mov [__r2],[_gyroYInt+2]
	mov [__r3],[_gyroYInt+3]
	mov A,10
X68:
	asr [__r0]
	rrc [__r1]
	rrc [__r2]
	rrc [__r3]
	dec A
	jnz X68
	mov [__r1],[__r3]
	mov [__r0],[__r2]
	mov A,[X-10]
	mov [__r3],A
	mov A,[__r0]
	mvi [__r3],A
	mov A,[__r1]
	mvi [__r3],A
	.dbline 401
	mov [__r0],[_gyroZInt]
	mov [__r1],[_gyroZInt+1]
	mov [__r2],[_gyroZInt+2]
	mov [__r3],[_gyroZInt+3]
	mov A,10
X69:
	asr [__r0]
	rrc [__r1]
	rrc [__r2]
	rrc [__r3]
	dec A
	jnz X69
	mov [__r1],[__r3]
	mov [__r0],[__r2]
	mov A,[X-12]
	mov [__r3],A
	mov A,[__r0]
	mvi [__r3],A
	mov A,[__r1]
	mvi [__r3],A
	.dbline 402
	mov A,[X-10]
	mov [__r1],A
	mvi A,[__r1]
	mov [__r0],A
	mvi A,[__r1]
	mov [__r3],A
	mov [__r2],[__r0]
	tst [__r2],-128
	jz X70
	mov [__r1],-1
	mov [__r0],-1
	jmp X71
X70:
	mov [__r1],0
	mov [__r0],0
X71:
	mov A,10
X72:
	asl [__r3]
	rlc [__r2]
	rlc [__r1]
	rlc [__r0]
	dec A
	jnz X72
	mov A,[__r3]
	sub [_gyroYInt+3],A
	mov A,[__r2]
	sbb [_gyroYInt+2],A
	mov A,[__r1]
	sbb [_gyroYInt+1],A
	mov A,[__r0]
	sbb [_gyroYInt],A
	.dbline 403
	mov A,[X-12]
	mov [__r1],A
	mvi A,[__r1]
	mov [__r0],A
	mvi A,[__r1]
	mov [__r3],A
	mov [__r2],[__r0]
	tst [__r2],-128
	jz X73
	mov [__r1],-1
	mov [__r0],-1
	jmp X74
X73:
	mov [__r1],0
	mov [__r0],0
X74:
	mov A,10
X75:
	asl [__r3]
	rlc [__r2]
	rlc [__r1]
	rlc [__r0]
	dec A
	jnz X75
	mov A,[__r3]
	sub [_gyroZInt+3],A
	mov A,[__r2]
	sbb [_gyroZInt+2],A
	mov A,[__r1]
	sbb [_gyroZInt+1],A
	mov A,[__r0]
	sbb [_gyroZInt],A
	.dbline 404
	cmp [_calibrated],0
	jnz L125
	.dbline 404
	.dbline 405
	mov A,[X-10]
	mov [__r1],A
	mov A,0
	mvi [__r1],A
	mvi [__r1],A
	.dbline 406
	mov A,[X-12]
	mov [__r1],A
	mov A,0
	mvi [__r1],A
	mvi [__r1],A
	.dbline 407
L125:
	.dbline -2
	.dbline 409
; 	lastButton = *button;
; 
; 	//Integrate the gyro data
; 	gyroYInt += gyroY;
; 	gyroZInt -= gyroZ;
; 
; 	//Round to the nearest pixel, and adjust the gyro integral
; 	*pixelYShift = (signed short)(gyroYInt>>10);
; 	*pixelZShift = (signed short)(gyroZInt>>10);
; 	gyroYInt-=(((signed long)*pixelYShift)<<10);
; 	gyroZInt-=(((signed long)*pixelZShift)<<10);
; 	if (calibrated==0) {
; 		*pixelYShift = 0;
; 		*pixelZShift = 0;
; 	}	
; 	
; }
L1:
	pop X
	.dbline 0 ; func end
	ret
	.dbsym l button -15 pc
	.dbsym l pixelZShift -13 pS
	.dbsym l pixelYShift -11 pS
	.dbsym l gyroZData -9 ps
	.dbsym l gyroYData -7 ps
	.dbsym l tempData -5 ps
	.dbend
	.area bss(ram, con, rel)
	.dbfile ./mousealgorithm.c
_motionTemp::
	.blkb 2
	.dbsym e motionTemp _motionTemp s