	cpu LMM
	.module diag.c
	.area data(ram, con, rel)
_mainmenu::
	.word L2
	.word L3
	.word L4
	.word L5
	.word L6
	.dbfile ./diag.c
	.dbsym e mainmenu _mainmenu A[10:5]pc
	.area data(ram, con, rel)
	.dbfile ./diag.c
	.area text(rom, con, rel)
	.dbfile ./diag.c
	.dbfunc e diag _diag fV
;            res -> X+0
_diag::
	.dbline -1
	push X
	mov X,SP
	add SP,1
	.dbline 27
; #include "pad.h"
; #include "main.h"
; #include "ed_host_int.h"
; #include "ed_slave_int.h"
; #include "ed_pad_int.h"
; #include "menu.h"
; #include "config.h"
; #include "util.h"
; #include "slave.h"
; 
; #include <math.h>
; #include <string.h>
; 
; unsigned char diag_about(void);
; unsigned char diag_baud(void);
; unsigned char diag_config(void);
; unsigned char diag_drive(void);
; unsigned char diag_configdrivemode(void);
; 
; void setmotor(unsigned char port, int dirpwm);
; 
; //const char *mainmenu[]={"Analog","Digital","DAC","Motors","QuadPhase","Servos",
; //						"Drive Mode","Config", "About", "Quit", ""};
; const char *mainmenu[]={"Drive Mode","Config", "About", "Quit", ""};
; 
; void diag(void)
; {
	.dbline 30
; 	signed char res;
; 
; 	res=0;
	mov [X+0],0
	xjmp L9
L8:
	.dbline 33
; 	
; 	while(res!=-1 && res!=3) // while not exit or Quit
; 		{
	.dbline 34
; 		res=menu("Diagnostics", mainmenu, res);
	mov A,[X+0]
	push A
	mov A,>_mainmenu
	push A
	mov A,<_mainmenu
	push A
	mov A,>L11
	push A
	mov A,<L11
	push A
	xcall _menu
	add SP,-5
	mov REG[0xd0],>__r0
	mov [X+0],A
	.dbline 36
; 
; 		if (res==0)
	cmp [X+0],0
	jnz L12
	.dbline 37
; 			diag_drive();
	xcall _diag_drive
	mov REG[0xd0],>__r0
L12:
	.dbline 38
; 		if (res==1)
	cmp [X+0],1
	jnz L14
	.dbline 39
; 			diag_config();
	xcall _diag_config
	mov REG[0xd0],>__r0
L14:
	.dbline 40
; 		if (res==2)
	cmp [X+0],2
	jnz L16
	.dbline 41
; 			diag_about();
	xcall _diag_about
	mov REG[0xd0],>__r0
L16:
	.dbline 43
L9:
	.dbline 32
	cmp [X+0],-1
	jz L18
	cmp [X+0],3
	jnz L8
L18:
	.dbline 44
; 		
; 		}
; 	pad_LcdClear();
	xcall _pad_LcdClear
	.dbline -2
	.dbline 45
; }
L7:
	add SP,-1
	pop X
	.dbline 0 ; func end
	ret
	.dbsym l res 0 C
	.dbend
	.dbfunc e diag_about _diag_about fc
_diag_about::
	.dbline -1
	.dbline 48
; 
; unsigned char diag_about(void)
; {
	.dbline 49
; 	pad_LcdClear();
	xcall _pad_LcdClear
	.dbline 51
; 	
; 	pad_LcdPrintC(0x00, 0x10, 'b', "OrcBoard/Pad (v4)");
	mov A,>L20
	push A
	mov A,<L20
	push A
	mov A,98
	push A
	mov A,16
	push A
	mov A,0
	push A
	xcall _pad_LcdPrintC
	.dbline 52
; 	pad_LcdPrintC(0x00, 0x20, 'B', "Developed by Edwin Olson");
	mov A,>L21
	push A
	mov A,<L21
	push A
	mov A,66
	push A
	mov A,32
	push A
	mov A,0
	push A
	xcall _pad_LcdPrintC
	add SP,-10
	.dbline 53
; 	pad_LcdPrintC(0x00, 0x28, 'B', "and MIT MASLab.");
	mov A,>L22
	push A
	mov A,<L22
	push A
	mov A,66
	push A
	mov A,40
	push A
	mov A,0
	push A
	xcall _pad_LcdPrintC
	.dbline 54
; 	pad_LcdPrintC(0x00, 0x38, 'B', "maslab.csail.mit.edu");
	mov A,>L23
	push A
	mov A,<L23
	push A
	mov A,66
	push A
	mov A,56
	push A
	mov A,0
	push A
	xcall _pad_LcdPrintC
	add SP,-10
	.dbline 56
; 
; 	pad_gets();
	xcall _pad_gets
	mov REG[0xd0],>__r0
	.dbline 57
; 	return 0;
	mov A,0
	.dbline -2
L19:
	.dbline 0 ; func end
	ret
	.dbend
	.area data(ram, con, rel)
	.dbfile ./diag.c
_configmenu::
	.word L24
	.word L25
	.word L5
	.word L6
	.dbsym e configmenu _configmenu A[8:4]pc
	.area data(ram, con, rel)
	.dbfile ./diag.c
	.area text(rom, con, rel)
	.dbfile ./diag.c
	.dbfunc e diag_config _diag_config fc
;            res -> X+0
_diag_config::
	.dbline -1
	push X
	mov X,SP
	add SP,3
	.dbline 63
; }
; 
; const char *configmenu[]={"Baud","DriveMode","Quit", ""};
; 
; unsigned char diag_config(void)
; {
	.dbline 66
; 	signed char res;
; 
; 	res=0;
	mov [X+0],0
	xjmp L28
L27:
	.dbline 69
; 	
; 	while (1)
; 	{
	.dbline 70
; 		res=menu("Config", configmenu, res);
	mov A,[X+0]
	push A
	mov A,>_configmenu
	push A
	mov A,<_configmenu
	push A
	mov A,>L3
	push A
	mov A,<L3
	push A
	xcall _menu
	add SP,-5
	mov REG[0xd0],>__r0
	mov [X+0],A
	.dbline 72
; 	
; 		if (res==2 || res<0)
	cmp [X+0],2
	jz L32
	tst [X+0],-128
	jz L30
L32:
	.dbline 73
; 			return 0;
	mov REG[0xd0],>__r0
	mov A,0
	xjmp L26
L30:
	.dbline 75
; 		
; 		switch(res)
	mov A,[X+0]
	mov [X+2],A
	mov [X+1],0
	tst [X+2],-128
	jz X2
	mov [X+1],-1
X2:
	cmp [X+1],0
	jnz X3
	cmp [X+2],0
	jz L36
X3:
	cmp [X+1],0
	jnz X4
	cmp [X+2],1
	jz L37
X4:
	xjmp L33
X0:
	.dbline 76
; 		{
L36:
	.dbline 78
; 			case 0:
; 				diag_baud();
	xcall _diag_baud
	mov REG[0xd0],>__r0
	.dbline 79
; 				break;
	xjmp L34
L37:
	.dbline 81
; 			case 1:
; 				diag_configdrivemode();
	xcall _diag_configdrivemode
	mov REG[0xd0],>__r0
	.dbline 82
; 				break;
L33:
L34:
	.dbline 84
L28:
	.dbline 68
	xjmp L27
X1:
	.dbline 85
; 		}
; 	}
; 	return 0;
	mov REG[0xd0],>__r0
	mov A,0
	.dbline -2
L26:
	add SP,-3
	pop X
	.dbline 0 ; func end
	ret
	.dbsym l res 0 C
	.dbend
	.area data(ram, con, rel)
	.dbfile ./diag.c
_baudmenu::
	.word L38
	.word L39
	.word L40
	.word L41
	.word L5
	.word L6
	.dbsym e baudmenu _baudmenu A[12:6]pc
	.area data(ram, con, rel)
	.dbfile ./diag.c
	.area text(rom, con, rel)
	.dbfile ./diag.c
	.dbfunc e diag_baud _diag_baud fc
;            res -> X+0
_diag_baud::
	.dbline -1
	push X
	mov X,SP
	add SP,1
	.dbline 91
; }
; 
; const char *baudmenu[]={"115.2","230.4", "250", "500","Quit", ""};
; 
; unsigned char diag_baud(void)
; {
	.dbline 94
; 	signed char res;
; 
; 	res=params.baud;
	mov REG[0xd0],>_params
	mov A,[_params+1]
	mov REG[0xd0],>__r0
	mov [X+0],A
	.dbline 96
; 	
; 	res=menu("Baud", baudmenu, res);
	mov A,[X+0]
	push A
	mov A,>_baudmenu
	push A
	mov A,<_baudmenu
	push A
	mov A,>L24
	push A
	mov A,<L24
	push A
	xcall _menu
	add SP,-5
	mov REG[0xd0],>__r0
	mov [X+0],A
	.dbline 98
; 
; 	if (res>3)
	mov A,3
	cmp A,[X+0]
	rrc A
	xor A,[X+0]
	xor A,3
	rlc A
	jnc L43
X5:
	.dbline 99
; 		return 0;
	mov REG[0xd0],>__r0
	mov A,0
	xjmp L42
L43:
	.dbline 101
; 		
; 	params.baud=res;
	mov REG[0xd0],>_params
	mov A,[X+0]
	mov [_params+1],A
	mov [_params],0
	tst [_params+1],-128
	jz X6
	mov [_params],-1
X6:
	.dbline 102
; 	savePrefs();
	xcall _savePrefs
	mov REG[0xd0],>__r0
	.dbline 103
; 	loadPrefs();
	xcall _loadPrefs
	.dbline 105
; 	
; 	return 0;
	mov REG[0xd0],>__r0
	mov A,0
	.dbline -2
L42:
	add SP,-1
	pop X
	.dbline 0 ; func end
	ret
	.dbsym l res 0 C
	.dbend
	.area data(ram, con, rel)
	.dbfile ./diag.c
_configdrivedmenu::
	.word L45
	.word L46
	.word L47
	.word L48
	.word L49
	.word L50
	.word L51
	.word L52
	.word L5
	.word L6
	.dbsym e configdrivedmenu _configdrivedmenu A[20:10]pc
	.area data(ram, con, rel)
	.dbfile ./diag.c
	.area text(rom, con, rel)
	.dbfile ./diag.c
	.dbfunc e diag_configdrivemode _diag_configdrivemode fc
;            res -> X+0
_diag_configdrivemode::
	.dbline -1
	push X
	mov X,SP
	add SP,1
	.dbline 111
; }
; 
; const char *configdrivedmenu[]={"LR","RL","lR","Rl","Lr","rL","lr","rl","Quit",""};
; 
; unsigned char diag_configdrivemode(void)
; {
	.dbline 114
; 	signed char res;
; 
; 	res=params.drivemode;
	mov REG[0xd0],>_params
	mov A,[_params+2]
	mov [X+0],A
	.dbline 116
; 	
; 	res=menu("Drive Mode", configdrivedmenu, res);
	mov A,[X+0]
	push A
	mov A,>_configdrivedmenu
	push A
	mov A,<_configdrivedmenu
	push A
	mov A,>L2
	push A
	mov A,<L2
	push A
	xcall _menu
	add SP,-5
	mov REG[0xd0],>__r0
	mov [X+0],A
	.dbline 117
; 	if (res>=0 && res<=7)
	tst [X+0],-128
	jnz L55
	mov A,[X+0]
	cmp A,7
	jz X7
	rrc A
	xor A,7
	xor A,[X+0]
	rlc A
	jnc L55
X7:
	.dbline 118
; 		{
	.dbline 119
; 		params.drivemode=res;
	mov A,[X+0]
	mov REG[0xd0],>_params
	mov [_params+2],A
	.dbline 120
; 		savePrefs();
	xcall _savePrefs
	mov REG[0xd0],>__r0
	.dbline 121
; 		loadPrefs();
	xcall _loadPrefs
	.dbline 122
; 		}
L55:
	.dbline 123
; 	return 0;
	mov REG[0xd0],>__r0
	mov A,0
	.dbline -2
L53:
	add SP,-1
	pop X
	.dbline 0 ; func end
	ret
	.dbsym l res 0 C
	.dbend
	.dbfunc e diag_drive _diag_drive fc
;           temp -> X+22
;         rscale -> X+18
;         lscale -> X+14
;          theta -> X+10
;              r -> X+6
;            cmd -> X+1
;              b -> X+0
_diag_drive::
	.dbline -1
	push X
	mov X,SP
	add SP,30
	.dbline 132
; }
; 
; #define PI  (3.1415926535f)
; #define HALFPI (PI/2)
; #define QUARTERPI (PI/4)
; #define RECIP_QUARTERPI (4/PI)
; 
; unsigned char diag_drive(void)
; {
	.dbline 139
; 	unsigned char b;
; 	unsigned char cmd[5];
; 	float theta, r;
; 	float lscale, rscale;
; 	float temp;
; 	
; 	pad_LcdClear();
	xcall _pad_LcdClear
	.dbline 142
; 			
; 	
; 	pad_waitButtonRelease();
	xcall _pad_waitButtonRelease
	xjmp L60
L59:
	.dbline 145
; 	
; 	while (1)
; 		{
	.dbline 146
; 		pad_LcdPrintC(0x00,0x08, 'D', "Drive Mode");
	mov A,>L2
	push A
	mov A,<L2
	push A
	mov A,68
	push A
	mov A,8
	push A
	mov A,0
	push A
	xcall _pad_LcdPrintC
	add SP,-5
	.dbline 148
; 
; 		b=pad_pollButtons();
	xcall _pad_pollButtons
	mov REG[0xd0],>__r0
	mov [X+0],A
	.dbline 149
; 		slave_requeststate();
	xcall _slave_requeststate
	.dbline 151
; 
; 		if (b&(PAD_MENU | PAD_STOP))
	tst [X+0],96
	jz L62
	.dbline 152
; 			{
	.dbline 153
; 			pad_waitButtonRelease();
	xcall _pad_waitButtonRelease
	.dbline 154
; 			return 0;
	mov REG[0xd0],>__r0
	mov A,0
	xjmp L58
L62:
	.dbline 157
; 			}
; 
; 		pad_LcdPrintC(0x08, 0x18, 'B',"Joy");
	mov A,>L64
	push A
	mov A,<L64
	push A
	mov A,66
	push A
	mov A,24
	push A
	mov A,8
	push A
	xcall _pad_LcdPrintC
	add SP,-5
	.dbline 159
; 		
; 		formatInt(pad_joyx, cmd, 4);
	mov A,4
	push A
	mov REG[0xd0],>__r0
	mov [__r1],X
	add [__r1],1
	mov A,7
	push A
	mov A,[__r1]
	push A
	mov REG[0xd0],>_pad_joyx
	mov A,[_pad_joyx]
	mov REG[0xd0],>__r0
	mov [__r1],A
	mov A,0
	push A
	mov A,[__r1]
	push A
	xcall _formatInt
	.dbline 160
; 		pad_LcdPrint(0x20,0x18, 'B', cmd);
	mov REG[0xd0],>__r0
	mov [__r1],X
	add [__r1],1
	mov A,7
	push A
	mov A,[__r1]
	push A
	mov A,66
	push A
	mov A,24
	push A
	mov A,32
	push A
	xcall _pad_LcdPrint
	add SP,-10
	.dbline 162
; 		
; 		formatInt(pad_joyy, cmd, 4);
	mov A,4
	push A
	mov REG[0xd0],>__r0
	mov [__r1],X
	add [__r1],1
	mov A,7
	push A
	mov A,[__r1]
	push A
	mov REG[0xd0],>_pad_joyy
	mov A,[_pad_joyy]
	mov REG[0xd0],>__r0
	mov [__r1],A
	mov A,0
	push A
	mov A,[__r1]
	push A
	xcall _formatInt
	.dbline 163
; 		pad_LcdPrint(0x50,0x18, 'B', cmd);
	mov REG[0xd0],>__r0
	mov [__r1],X
	add [__r1],1
	mov A,7
	push A
	mov A,[__r1]
	push A
	mov A,66
	push A
	mov A,24
	push A
	mov A,80
	push A
	xcall _pad_LcdPrint
	add SP,-10
	.dbline 165
; 		
; 		pad_LcdPrintC(0x08, 0x28, 'B',"PWM");
	mov A,>L65
	push A
	mov A,<L65
	push A
	mov A,66
	push A
	mov A,40
	push A
	mov A,8
	push A
	xcall _pad_LcdPrintC
	.dbline 167
; 		
; 		formatInt(getmotoractualsignedpwm(0), cmd, 4);
	mov A,0
	push A
	xcall _getmotoractualsignedpwm
	add SP,-6
	mov A,4
	push A
	mov REG[0xd0],>__r0
	mov [__r3],X
	add [__r3],1
	mov A,7
	push A
	mov A,[__r3]
	push A
	mov A,[__r0]
	push A
	mov A,[__r1]
	push A
	xcall _formatInt
	.dbline 168
; 		pad_LcdPrint(0x20,0x28, 'B', cmd);
	mov REG[0xd0],>__r0
	mov [__r1],X
	add [__r1],1
	mov A,7
	push A
	mov A,[__r1]
	push A
	mov A,66
	push A
	mov A,40
	push A
	mov A,32
	push A
	xcall _pad_LcdPrint
	add SP,-10
	.dbline 170
; 		
; 		formatInt(getmotoractualsignedpwm(2), cmd, 4);
	mov A,2
	push A
	xcall _getmotoractualsignedpwm
	mov A,4
	push A
	mov REG[0xd0],>__r0
	mov [__r3],X
	add [__r3],1
	mov A,7
	push A
	mov A,[__r3]
	push A
	mov A,[__r0]
	push A
	mov A,[__r1]
	push A
	xcall _formatInt
	add SP,-6
	.dbline 171
; 		pad_LcdPrint(0x50,0x28, 'B', cmd);
	mov REG[0xd0],>__r0
	mov [__r1],X
	add [__r1],1
	mov A,7
	push A
	mov A,[__r1]
	push A
	mov A,66
	push A
	mov A,40
	push A
	mov A,80
	push A
	xcall _pad_LcdPrint
	.dbline 173
; 
; 		pad_LcdPrintC(0x08,0x38,'B',"mA");
	mov A,>L66
	push A
	mov A,<L66
	push A
	mov A,66
	push A
	mov A,56
	push A
	mov A,8
	push A
	xcall _pad_LcdPrintC
	add SP,-10
	.dbline 175
; 
; 		formatInt(getmotorcurrent(0),cmd ,5);	
	mov A,0
	push A
	xcall _getmotorcurrent
	mov A,5
	push A
	mov REG[0xd0],>__r0
	mov [__r3],X
	add [__r3],1
	mov A,7
	push A
	mov A,[__r3]
	push A
	mov A,[__r0]
	push A
	mov A,[__r1]
	push A
	xcall _formatInt
	add SP,-6
	.dbline 176
; 		pad_LcdPrint(0x18,0x38, 'B', cmd);
	mov REG[0xd0],>__r0
	mov [__r1],X
	add [__r1],1
	mov A,7
	push A
	mov A,[__r1]
	push A
	mov A,66
	push A
	mov A,56
	push A
	mov A,24
	push A
	xcall _pad_LcdPrint
	.dbline 178
; 
; 		formatInt(getmotorcurrent(2),cmd ,5);	
	mov A,2
	push A
	xcall _getmotorcurrent
	add SP,-6
	mov A,5
	push A
	mov REG[0xd0],>__r0
	mov [__r3],X
	add [__r3],1
	mov A,7
	push A
	mov A,[__r3]
	push A
	mov A,[__r0]
	push A
	mov A,[__r1]
	push A
	xcall _formatInt
	.dbline 179
; 		pad_LcdPrint(0x48,0x38, 'B', cmd);
	mov REG[0xd0],>__r0
	mov [__r1],X
	add [__r1],1
	mov A,7
	push A
	mov A,[__r1]
	push A
	mov A,66
	push A
	mov A,56
	push A
	mov A,72
	push A
	xcall _pad_LcdPrint
	add SP,-10
	.dbline 181
; 
; 		theta=atan2(128-pad_joyy, pad_joyx-128);
	mov REG[0xd0],>_pad_joyx
	mov A,[_pad_joyx]
	mov REG[0xd0],>__r0
	mov [__r1],A
	mov [__r0],0
	sub [__r1],-128
	sbb [__r0],0
	mov A,[__r1]
	mov [__r3],A
	mov A,[__r0]
	mov [__r2],A
	tst [__r2],-128
	jz X9
	mov [__r1],-1
	mov [__r0],-1
	jmp X10
X9:
	mov REG[0xd0],>__r0
	mov [__r1],0
	mov [__r0],0
X10:
	mov REG[0xd0],>__r0
	mov A,[__r0]
	push A
	mov A,[__r1]
	push A
	mov A,[__r2]
	push A
	mov A,[__r3]
	push A
	xcall __long2fp
	pop A
	mov [__r3],A
	pop A
	mov [__r2],A
	pop A
	mov [__r1],A
	pop A
	push A
	mov A,[__r1]
	push A
	mov A,[__r2]
	push A
	mov A,[__r3]
	push A
	mov REG[0xd0],>_pad_joyy
	mov A,[_pad_joyy]
	mov REG[0xd0],>__r0
	mov [__r1],A
	mov [__r0],0
	mov A,-128
	sub A,[__r1]
	mov [__r1],A
	mov A,0
	sbb A,[__r0]
	mov [__r0],A
	mov A,[__r1]
	mov [__r3],A
	mov A,[__r0]
	mov [__r2],A
	tst [__r2],-128
	jz X11
	mov [__r1],-1
	mov [__r0],-1
	jmp X12
X11:
	mov REG[0xd0],>__r0
	mov [__r1],0
	mov [__r0],0
X12:
	mov REG[0xd0],>__r0
	mov A,[__r0]
	push A
	mov A,[__r1]
	push A
	mov A,[__r2]
	push A
	mov A,[__r3]
	push A
	xcall __long2fp
	pop A
	mov [__r3],A
	pop A
	mov [__r2],A
	pop A
	mov [__r1],A
	pop A
	push A
	mov A,[__r1]
	push A
	mov A,[__r2]
	push A
	mov A,[__r3]
	push A
	xcall _atan2
	add SP,-8
	mov REG[0xd0],>__r0
	mov A,[__r0]
	mov [X+10],A
	mov A,[__r1]
	mov [X+11],A
	mov A,[__r2]
	mov [X+12],A
	mov A,[__r3]
	mov [X+13],A
	.dbline 185
; 		
; 		// note: multiplier of 2.2 gives a little "gain" over the nominal 2.0
; 		// in order to make it a bit easier to go full blast.
; 		r=2.2f*sqrt(pow(pad_joyy-128,2) + pow(pad_joyx-128,2));
	mov A,64
	push A
	mov A,0
	push A
	push A
	push A
	mov REG[0xd0],>_pad_joyy
	mov A,[_pad_joyy]
	mov REG[0xd0],>__r0
	mov [__r1],A
	mov [__r0],0
	sub [__r1],-128
	sbb [__r0],0
	mov A,[__r1]
	mov [__r3],A
	mov A,[__r0]
	mov [__r2],A
	tst [__r2],-128
	jz X13
	mov [__r1],-1
	mov [__r0],-1
	jmp X14
X13:
	mov REG[0xd0],>__r0
	mov [__r1],0
	mov [__r0],0
X14:
	mov REG[0xd0],>__r0
	mov A,[__r0]
	push A
	mov A,[__r1]
	push A
	mov A,[__r2]
	push A
	mov A,[__r3]
	push A
	xcall __long2fp
	pop A
	mov [__r3],A
	pop A
	mov [__r2],A
	pop A
	mov [__r1],A
	pop A
	push A
	mov A,[__r1]
	push A
	mov A,[__r2]
	push A
	mov A,[__r3]
	push A
	xcall _pow
	add SP,-8
	mov REG[0xd0],>__r0
	mov A,[__r0]
	mov [X+26],A
	mov A,[__r1]
	mov [X+27],A
	mov A,[__r2]
	mov [X+28],A
	mov A,[__r3]
	mov [X+29],A
	mov A,64
	push A
	mov A,0
	push A
	push A
	push A
	mov REG[0xd0],>_pad_joyx
	mov A,[_pad_joyx]
	mov REG[0xd0],>__r0
	mov [__r5],A
	mov [__r4],0
	sub [__r5],-128
	sbb [__r4],0
	mov A,[__r5]
	mov [__r7],A
	mov A,[__r4]
	mov [__r6],A
	tst [__r6],-128
	jz X15
	mov [__r5],-1
	mov [__r4],-1
	jmp X16
X15:
	mov REG[0xd0],>__r0
	mov [__r5],0
	mov [__r4],0
X16:
	mov REG[0xd0],>__r0
	mov A,[__r4]
	push A
	mov A,[__r5]
	push A
	mov A,[__r6]
	push A
	mov A,[__r7]
	push A
	xcall __long2fp
	pop A
	mov [__r7],A
	pop A
	mov [__r6],A
	pop A
	mov [__r5],A
	pop A
	push A
	mov A,[__r5]
	push A
	mov A,[__r6]
	push A
	mov A,[__r7]
	push A
	xcall _pow
	add SP,-8
	mov REG[0xd0],>__r0
	mov A,[__r0]
	push A
	mov A,[__r1]
	push A
	mov A,[__r2]
	push A
	mov A,[__r3]
	push A
	mov A,[X+26]
	push A
	mov A,[X+27]
	push A
	mov A,[X+28]
	push A
	mov A,[X+29]
	push A
	xcall __fpadd
	pop A
	mov [__r3],A
	pop A
	mov [__r2],A
	pop A
	mov [__r1],A
	pop A
	add SP,-4
	push A
	mov A,[__r1]
	push A
	mov A,[__r2]
	push A
	mov A,[__r3]
	push A
	xcall _sqrt
	add SP,-4
	mov REG[0xd0],>__r0
	mov A,[__r0]
	push A
	mov A,[__r1]
	push A
	mov A,[__r2]
	push A
	mov A,[__r3]
	push A
	mov A,64
	push A
	mov A,12
	push A
	mov A,-52
	push A
	mov A,-51
	push A
	xcall __fpmul
	pop A
	mov [X+9],A
	pop A
	mov [X+8],A
	pop A
	mov [X+7],A
	pop A
	mov [X+6],A
	add SP,-4
	.dbline 186
; 		if (r>255.0f)	// saturate	
	mov A,67
	push A
	mov A,127
	push A
	mov A,0
	push A
	push A
	mov A,[X+6]
	push A
	mov A,[X+7]
	push A
	mov A,[X+8]
	push A
	mov A,[X+9]
	push A
	xcall __fpcmp
	add SP,-8
	cmp A,0
	jz L67
	cmp A,1
	jnz L67
	.dbline 187
; 			r=255.0f;
	mov [X+6],67
	mov [X+7],127
	mov [X+8],0
	mov [X+9],0
L67:
	.dbline 188
; 		if (r<20.0f)	// create a "dead" zone in the middle
	mov A,65
	push A
	mov A,-96
	push A
	mov A,0
	push A
	push A
	mov A,[X+6]
	push A
	mov A,[X+7]
	push A
	mov A,[X+8]
	push A
	mov A,[X+9]
	push A
	mov REG[0xd0],>__r0
	xcall __fpcmp
	add SP,-8
	cmp A,0
	jz L69
	cmp A,1
	jz L69
	.dbline 189
; 			r=0.0f;
	mov [X+6],0
	mov [X+7],0
	mov [X+8],0
	mov [X+9],0
L69:
	.dbline 192
; 		
; 		// give other clients a chance. floating point is slow.
; 		for (b=0;b<20;b++)
	mov [X+0],0
	xjmp L74
L71:
	.dbline 193
	xcall _idle
L72:
	.dbline 192
	inc [X+0]
L74:
	.dbline 192
	cmp [X+0],20
	jc L71
X19:
	.dbline 196
; 			idle();
; 	
; 		//theta is [-PI, PI] (spec of atan2)
; 		if (theta>=0 && theta<=HALFPI)
	mov A,0
	push A
	push A
	push A
	push A
	mov A,[X+10]
	push A
	mov A,[X+11]
	push A
	mov A,[X+12]
	push A
	mov A,[X+13]
	push A
	mov REG[0xd0],>__r0
	xcall __fpcmp
	add SP,-8
	cmp A,-1
	jz L75
	mov A,63
	push A
	mov A,-55
	push A
	mov A,15
	push A
	mov A,-37
	push A
	mov A,[X+10]
	push A
	mov A,[X+11]
	push A
	mov A,[X+12]
	push A
	mov A,[X+13]
	push A
	xcall __fpcmp
	add SP,-8
	cmp A,1
	jz L75
	.dbline 197
; 		{
	.dbline 198
; 			rscale=-1.0f+theta*RECIP_QUARTERPI;
	mov A,[X+10]
	push A
	mov A,[X+11]
	push A
	mov A,[X+12]
	push A
	mov A,[X+13]
	push A
	mov A,63
	push A
	mov A,-94
	push A
	mov A,-7
	push A
	mov A,-125
	push A
	xcall __fpmul
	pop A
	mov [__r3],A
	pop A
	mov [__r2],A
	pop A
	mov [__r1],A
	pop A
	mov [__r0],A
	add SP,-4
	mov A,-65
	push A
	mov A,-128
	push A
	mov A,0
	push A
	push A
	mov A,[__r0]
	push A
	mov A,[__r1]
	push A
	mov A,[__r2]
	push A
	mov A,[__r3]
	push A
	xcall __fpadd
	pop A
	mov [X+21],A
	pop A
	mov [X+20],A
	pop A
	mov [X+19],A
	pop A
	mov [X+18],A
	add SP,-4
	.dbline 199
; 			lscale=1.0f;
	mov [X+14],63
	mov [X+15],-128
	mov [X+16],0
	mov [X+17],0
	.dbline 200
; 		}
	xjmp L76
L75:
	.dbline 201
; 		else if (theta>HALFPI)
	mov A,63
	push A
	mov A,-55
	push A
	mov A,15
	push A
	mov A,-37
	push A
	mov A,[X+10]
	push A
	mov A,[X+11]
	push A
	mov A,[X+12]
	push A
	mov A,[X+13]
	push A
	mov REG[0xd0],>__r0
	xcall __fpcmp
	add SP,-8
	cmp A,0
	jz L77
	cmp A,1
	jnz L77
	.dbline 202
; 		{
	.dbline 203
; 			rscale=1.0f;
	mov [X+18],63
	mov [X+19],-128
	mov [X+20],0
	mov [X+21],0
	.dbline 204
; 			lscale=1.0f-(theta-HALFPI)*RECIP_QUARTERPI;
	mov A,63
	push A
	mov A,-55
	push A
	mov A,15
	push A
	mov A,-37
	push A
	mov A,[X+10]
	push A
	mov A,[X+11]
	push A
	mov A,[X+12]
	push A
	mov A,[X+13]
	push A
	xcall __fpsub
	pop A
	mov [__r3],A
	pop A
	mov [__r2],A
	pop A
	mov [__r1],A
	pop A
	add SP,-4
	push A
	mov A,[__r1]
	push A
	mov A,[__r2]
	push A
	mov A,[__r3]
	push A
	mov A,63
	push A
	mov A,-94
	push A
	mov A,-7
	push A
	mov A,-125
	push A
	xcall __fpmul
	pop A
	mov [__r3],A
	pop A
	mov [__r2],A
	pop A
	mov [__r1],A
	pop A
	add SP,-4
	push A
	mov A,[__r1]
	push A
	mov A,[__r2]
	push A
	mov A,[__r3]
	push A
	mov A,63
	push A
	mov A,-128
	push A
	mov A,0
	push A
	push A
	xcall __fpsub
	pop A
	mov [X+17],A
	pop A
	mov [X+16],A
	pop A
	mov [X+15],A
	pop A
	mov [X+14],A
	add SP,-4
	.dbline 205
; 		}
	xjmp L78
L77:
	.dbline 206
; 		else if (theta>=-HALFPI)
	mov A,-65
	push A
	mov A,-55
	push A
	mov A,15
	push A
	mov A,-37
	push A
	mov A,[X+10]
	push A
	mov A,[X+11]
	push A
	mov A,[X+12]
	push A
	mov A,[X+13]
	push A
	mov REG[0xd0],>__r0
	xcall __fpcmp
	add SP,-8
	cmp A,-1
	jz L79
	.dbline 207
; 		{
	.dbline 208
; 			rscale=-1.0f;
	mov [X+18],-65
	mov [X+19],-128
	mov [X+20],0
	mov [X+21],0
	.dbline 209
; 			lscale=-1.0f+(theta+HALFPI)*RECIP_QUARTERPI;
	mov A,63
	push A
	mov A,-55
	push A
	mov A,15
	push A
	mov A,-37
	push A
	mov A,[X+10]
	push A
	mov A,[X+11]
	push A
	mov A,[X+12]
	push A
	mov A,[X+13]
	push A
	xcall __fpadd
	pop A
	mov [__r3],A
	pop A
	mov [__r2],A
	pop A
	mov [__r1],A
	pop A
	add SP,-4
	push A
	mov A,[__r1]
	push A
	mov A,[__r2]
	push A
	mov A,[__r3]
	push A
	mov A,63
	push A
	mov A,-94
	push A
	mov A,-7
	push A
	mov A,-125
	push A
	xcall __fpmul
	pop A
	mov [__r3],A
	pop A
	mov [__r2],A
	pop A
	mov [__r1],A
	pop A
	mov [__r0],A
	add SP,-4
	mov A,-65
	push A
	mov A,-128
	push A
	mov A,0
	push A
	push A
	mov A,[__r0]
	push A
	mov A,[__r1]
	push A
	mov A,[__r2]
	push A
	mov A,[__r3]
	push A
	xcall __fpadd
	pop A
	mov [X+17],A
	pop A
	mov [X+16],A
	pop A
	mov [X+15],A
	pop A
	mov [X+14],A
	add SP,-4
	.dbline 210
; 		}
	xjmp L80
L79:
	.dbline 212
; 		else
; 		{
	.dbline 213
; 			rscale=1.0f-(theta+PI)*RECIP_QUARTERPI;
	mov REG[0xd0],>__r0
	mov A,64
	push A
	mov A,73
	push A
	mov A,15
	push A
	mov A,-37
	push A
	mov A,[X+10]
	push A
	mov A,[X+11]
	push A
	mov A,[X+12]
	push A
	mov A,[X+13]
	push A
	xcall __fpadd
	pop A
	mov [__r3],A
	pop A
	mov [__r2],A
	pop A
	mov [__r1],A
	pop A
	add SP,-4
	push A
	mov A,[__r1]
	push A
	mov A,[__r2]
	push A
	mov A,[__r3]
	push A
	mov A,63
	push A
	mov A,-94
	push A
	mov A,-7
	push A
	mov A,-125
	push A
	xcall __fpmul
	pop A
	mov [__r3],A
	pop A
	mov [__r2],A
	pop A
	mov [__r1],A
	pop A
	add SP,-4
	push A
	mov A,[__r1]
	push A
	mov A,[__r2]
	push A
	mov A,[__r3]
	push A
	mov A,63
	push A
	mov A,-128
	push A
	mov A,0
	push A
	push A
	xcall __fpsub
	pop A
	mov [X+21],A
	pop A
	mov [X+20],A
	pop A
	mov [X+19],A
	pop A
	mov [X+18],A
	add SP,-4
	.dbline 214
; 			lscale=-1.0f;
	mov [X+14],-65
	mov [X+15],-128
	mov [X+16],0
	mov [X+17],0
	.dbline 215
; 		}
L80:
L78:
L76:
	.dbline 218
; 		
; 		// give other clients a chance. floating point is slow.
; 		for (b=0;b<20;b++)
	mov [X+0],0
	xjmp L84
L81:
	.dbline 219
	xcall _idle
L82:
	.dbline 218
	inc [X+0]
L84:
	.dbline 218
	cmp [X+0],20
	jc L81
X24:
	.dbline 221
; 			idle();
; 		
; 		if (params.drivemode&DRIVEMODE_LEFTINV)
	mov REG[0xd0],>_params
	tst [_params+2],2
	jz L85
	.dbline 222
; 			lscale=-lscale;
	xor [X+14],-128
L85:
	.dbline 223
; 		if (params.drivemode&DRIVEMODE_RIGHTINV)
	mov REG[0xd0],>_params
	tst [_params+2],4
	jz L88
	.dbline 224
; 			rscale=-rscale;
	xor [X+18],-128
L88:
	.dbline 225
; 		if (params.drivemode&DRIVEMODE_SWAPLR)
	mov REG[0xd0],>_params
	tst [_params+2],1
	jz L91
	.dbline 226
; 		{
	.dbline 227
; 			temp=rscale;
	mov A,[X+18]
	mov [X+22],A
	mov A,[X+19]
	mov [X+23],A
	mov A,[X+20]
	mov [X+24],A
	mov A,[X+21]
	mov [X+25],A
	.dbline 228
; 			rscale=lscale;
	mov A,[X+14]
	mov [X+18],A
	mov A,[X+15]
	mov [X+19],A
	mov A,[X+16]
	mov [X+20],A
	mov A,[X+17]
	mov [X+21],A
	.dbline 229
; 			lscale=temp;
	mov A,[X+22]
	mov [X+14],A
	mov A,[X+23]
	mov [X+15],A
	mov A,[X+24]
	mov [X+16],A
	mov A,[X+25]
	mov [X+17],A
	.dbline 230
; 		}
L91:
	.dbline 232
; 		
; 		setmotor(0, (int) (lscale*r));
	mov REG[0xd0],>__r0
	mov A,[X+6]
	push A
	mov A,[X+7]
	push A
	mov A,[X+8]
	push A
	mov A,[X+9]
	push A
	mov A,[X+14]
	push A
	mov A,[X+15]
	push A
	mov A,[X+16]
	push A
	mov A,[X+17]
	push A
	xcall __fpmul
	pop A
	mov [__r3],A
	pop A
	mov [__r2],A
	pop A
	mov [__r1],A
	pop A
	add SP,-4
	push A
	mov A,[__r1]
	push A
	mov A,[__r2]
	push A
	mov A,[__r3]
	push A
	xcall __fp2long
	pop A
	mov [__r1],A
	pop A
	add SP,-2
	push A
	mov A,[__r1]
	push A
	mov A,0
	push A
	xcall _setmotor
	add SP,-3
	.dbline 233
; 		setmotor(2, (int) (rscale*r));
	mov REG[0xd0],>__r0
	mov A,[X+6]
	push A
	mov A,[X+7]
	push A
	mov A,[X+8]
	push A
	mov A,[X+9]
	push A
	mov A,[X+18]
	push A
	mov A,[X+19]
	push A
	mov A,[X+20]
	push A
	mov A,[X+21]
	push A
	xcall __fpmul
	pop A
	mov [__r3],A
	pop A
	mov [__r2],A
	pop A
	mov [__r1],A
	pop A
	add SP,-4
	push A
	mov A,[__r1]
	push A
	mov A,[__r2]
	push A
	mov A,[__r3]
	push A
	xcall __fp2long
	pop A
	mov [__r1],A
	pop A
	add SP,-2
	push A
	mov A,[__r1]
	push A
	mov A,2
	push A
	xcall _setmotor
	add SP,-3
	.dbline 238
; 		
; 		//setmotor(0, 5*(pad_joyy+pad_joyx-256)/2);
; 		//setmotor(2, -5*(pad_joyy-pad_joyx)/2);
; 
; 		for (b=0;b<50;b++)
	mov [X+0],0
	xjmp L97
L94:
	.dbline 239
	xcall _idle
L95:
	.dbline 238
	inc [X+0]
L97:
	.dbline 238
	cmp [X+0],50
	jc L94
X25:
	.dbline 240
L60:
	.dbline 144
	xjmp L59
X8:
	.dbline 241
; 			idle();
; 		}
; 	return 0;
	mov REG[0xd0],>__r0
	mov A,0
	.dbline -2
L58:
	add SP,-30
	pop X
	.dbline 0 ; func end
	ret
	.dbsym l temp 22 D
	.dbsym l rscale 18 D
	.dbsym l lscale 14 D
	.dbsym l theta 10 D
	.dbsym l r 6 D
	.dbsym l cmd 1 A[5:5]c
	.dbsym l b 0 c
	.dbend
	.area lit(rom, con, rel)
L66:
	.byte 'm,'A,0
L65:
	.byte 'P,'W,'M,0
L64:
	.byte 'J,'o,'y,0
L52:
	.byte 'r,'l,0
L51:
	.byte 'l,'r,0
L50:
	.byte 'r,'L,0
L49:
	.byte 'L,'r,0
L48:
	.byte 'R,'l,0
L47:
	.byte 'l,'R,0
L46:
	.byte 'R,'L,0
L45:
	.byte 'L,'R,0
L41:
	.byte 53,48,48,0
L40:
	.byte 50,53,48,0
L39:
	.byte 50,51,48,46,52,0
L38:
	.byte 49,49,53,46,50,0
L25:
	.byte 'D,'r,'i,'v,'e,'M,'o,'d,'e,0
L24:
	.byte 'B,'a,'u,'d,0
L23:
	.byte 'm,'a,'s,'l,'a,'b,46,'c,'s,'a,'i,'l,46,'m,'i,'t
	.byte 46,'e,'d,'u,0
L22:
	.byte 'a,'n,'d,32,'M,'I,'T,32,'M,'A,'S,'L,'a,'b,46,0
L21:
	.byte 'D,'e,'v,'e,'l,'o,'p,'e,'d,32,'b,'y,32,'E,'d,'w
	.byte 'i,'n,32,'O,'l,'s,'o,'n,0
L20:
	.byte 'O,'r,'c,'B,'o,'a,'r,'d,47,'P,'a,'d,32,40,'v,52
	.byte 41,0
L11:
	.byte 'D,'i,'a,'g,'n,'o,'s,'t,'i,'c,'s,0
L6:
	.byte 0
L5:
	.byte 'Q,'u,'i,'t,0
L4:
	.byte 'A,'b,'o,'u,'t,0
L3:
	.byte 'C,'o,'n,'f,'i,'g,0
L2:
	.byte 'D,'r,'i,'v,'e,32,'M,'o,'d,'e,0
