; Motor Control for Turning Inspirations Art Installation
; Copyright 2007, Paul Stoffregen



.equ    locat, 0x8000           ;Location for this program

.equ    reg0, 0
.equ    reg1, 1
.equ    reg2, 2
.equ    reg3, 3
.equ    reg4, 4


;I/O usage
.equ	port_motor1, 0xF800		;port A
.equ	port_rotary_encoders, 0xF801	;port B
.equ	port_motor2, 0xF802		;port C
.equ	port_abc_pgm, 0xF803		;configure in/out of all three ports
.equ	port_abc_pgm_value, 0x82	;A=out, B=in, C=out
.equ	port_motor3, 0xF900		;port D
.equ	port_e, 0xF901			;port E - LEDs
.equ	port_motor4, 0xF902		;port F
.equ	port_def_pgm, 0xF903		;configure in/out of all three ports
.equ	port_def_pgm_value, 0x80	;D=out, E=out, F=out
.equ	rxen_pin, 0x90			;P1.0 low to enable receiver
.equ	txen_pin, 0x91			;P1.1 low to enable transmitter
.equ	optical_1_pin, 0x92		;P1.2
.equ	optical_2_pin, 0x93		;P1.3
.equ	optical_3_pin, 0x94		;P1.4
.equ	optical_4_pin, 0x95		;P1.5
.equ	dip_switch_0, 0x96		;P1.6
.equ	dip_switch_1, 0x97		;P1.7
.equ	dip_switch_2, 0xB2		;P3.2/INT0
.equ	dip_switch_3, 0xB3		;P3.3/INT1

;PAULMON2 routines
.equ    cout, 0x0030		;print byte in acc to serial port
.equ    cin, 0x0032 		;get byte from serial port into acc
.equ    phex, 0x0034 		;print acc in hex (2 digits)
.equ    phex16, 0x0036		;print dptr in hex (4 digits)
.equ    pstr, 0x0038 		;print a string @dptr
.equ	esc, 0x003E		;paulmon2's check for esc key
.equ    newline, 0x0048		;print a newline (CR and LF)
.equ    pint8u, 0x004D		;print acc as unsigned int (0 to 255)
.equ    pint16u, 0x0053		;print dptr as unsigned int (0 to 65535)


;internal ram usage:
.equ	motor1_position, 0x08		;position (in motor steps), 0 to 199
.equ	motor2_position, 0x09
.equ	motor3_position, 0x0A
.equ	motor4_position, 0x0B
.equ	rotary1_position, 0x0C		;position via sensors, 0 to 95
.equ	rotary2_position, 0x0D
.equ	rotary3_position, 0x0E
.equ	rotary4_position, 0x0F
.equ	flags1, 0x20			;bit variables (see below)
.equ	flags2, 0x21
.equ	motor1_step_state, 0x2C		;current half-step index, 0 to 7
.equ	motor2_step_state, 0x2D
.equ	motor3_step_state, 0x2E
.equ	motor4_step_state, 0x2F
.equ	motor1_speed, 0x30		;0 = stop, 255 = max speed
.equ	motor2_speed, 0x31
.equ	motor3_speed, 0x32
.equ	motor4_speed, 0x33
.equ	delay1_count, 0x34
.equ	delay2_count, 0x35
.equ	delay3_count, 0x36
.equ	delay4_count, 0x37
.equ	motor1_spd_state, 0x38		;speed state, each rollover is 1 step
.equ	motor2_spd_state, 0x39
.equ	motor3_spd_state, 0x3A
.equ	motor4_spd_state, 0x3B
.equ	optical_1_history, 0x3C		;holds previous 8 optical sensor readings
.equ	optical_2_history, 0x3D
.equ	optical_3_history, 0x3E
.equ	optical_4_history, 0x3F
.equ	encoder_prev_lsn, 0x40		;holds the previous rotary encoder values
.equ	encoder_prev_msn, 0x41
.equ	input_buf_ptr, 0x42		;points past end of data in input_buf
.equ	my_address, 0x43		;holds 2's complement of the address
.equ	idle_task_next, 0x44
.equ	tracking_err_count_1, 0x45	;number of times the rotary != motor position
.equ	tracking_err_count_2, 0x46
.equ	tracking_err_count_3, 0x47
.equ	tracking_err_count_4, 0x48

.equ    input_buf, 0x80
.equ	input_buf_size, 64
.equ    stack, 0xC0



;internal bit/flags
.equ	motor1_dir, 0			;0 = CW, 1 = CCW
.equ	motor2_dir, 1
.equ	motor3_dir, 2
.equ	motor4_dir, 3
.equ	motor1_enabled, 4		;0 = no power (spin free), 1 = powered
.equ	motor2_enabled, 5
.equ	motor3_enabled, 6
.equ	motor4_enabled, 7
.equ	tracking_check_reqd_1, 8
.equ	tracking_check_reqd_2, 9
.equ	tracking_check_reqd_3, 10
.equ	tracking_check_reqd_4, 11

.org	locat

	ljmp	startup



;*************************************************************************
;**									**
;**	Interrupt Routine, runs at 2000 Hz, updates motors and		**
;**	reads all sensors, and implements software timers		**
;**									**
;*************************************************************************

.org	locat+0x23

	; the transmit code enables this interrupt after placing the
	; last byte into the transmitter.  When we get this interrupt,
	; we must shut the rs-485 transmit off ASAP.  This interrupt
	; is high priority, so it can interrup the other one if necessary
serial_isr:
	setb	txen_pin	;turn off the rs-485 transmitter
	clr	rxen_pin
	clr	es		;disable this interrupt
	reti



.org	locat+0x2B

;--------------------------------;
;  Enter Interrupt Routine       ;
;--------------------------------;

timer2_isr:					;5
	push	psw				;2
	mov	psw, #00001000b			;2
	push	acc				;2
	push	b				;2
	push	dpl				;2
	push	dph				;2
	clr	tf2				;1
	clr	exf2				;1	19 cycles to enter

	;Register Bank #1 Usage:
	;-----------------------
	;r0 = motor1_position		position (in motor steps), 0 to 199
	;r1 = motor2_position
	;r2 = motor3_position
	;r3 = motor4_position
	;r4 = rotary1_position		position via sensors, 0 to 95
	;r5 = rotary2_position
	;r6 = rotary3_position
	;r7 = rotary4_position

;--------------------------------;
;  Read Rotary Encoders          ;
;--------------------------------;

t2_rotary_enc:					;worst	typical
	mov	dptr, #port_rotary_encoders	;2	2
	movx	a, @dptr			;2	2
	mov	b, a				;1	1
	mov	dptr, #table_encoder_change	;2	2
t2_enc_1and2:
	swap	a				;1	1
	anl	a, #0xF0			;1	1
	orl	a, encoder_prev_lsn		;1	1
	movc	a, @a+dptr			;1	1
t2_enc_1_inc:
	jnb	acc.1, t2_enc_1_dec		;2	2
	setb	tracking_check_reqd_1
	inc	r4				;1		rotary1_position = r4
	cjne	r4, #96, t2_enc_1_dec		;2
	mov	r4, #0				;1
t2_enc_1_dec:
	jnb	acc.0, t2_enc_2_inc		;2	2
	setb	tracking_check_reqd_1
	dec	r4
	cjne	r4, #255, t2_enc_2_inc
	mov	r4, #95
t2_enc_2_inc:
	jnb	acc.3, t2_enc_2_dec		;2	2
	setb	tracking_check_reqd_2
	inc	r5				;1		rotary2_position = r5
	cjne	r5, #96, t2_enc_2_dec		;2
	mov	r5, #0				;1
t2_enc_2_dec:
	jnb	acc.2, t2_enc_3and4		;2	2
	setb	tracking_check_reqd_2
	dec	r5
	cjne	r5, #255, t2_enc_3and4
	mov	r5, #95
t2_enc_3and4:
	mov	a, b				;1	1
	anl	a, #0xF0			;1	1
	orl	a, encoder_prev_msn		;1	1
	movc	a, @a+dptr			;1	1
t2_enc_3_inc:
	jnb	acc.1, t2_enc_3_dec		;2	2
	setb	tracking_check_reqd_3
	inc	r6				;1		rotary3_position = r6
	cjne	r6, #96, t2_enc_3_dec		;2
	mov	r6, #0				;1
t2_enc_3_dec:
	jnb	acc.0, t2_enc_4_inc		;2	2
	setb	tracking_check_reqd_3
	dec	r6
	cjne	r6, #255, t2_enc_4_inc
	mov	r6, #95
t2_enc_4_inc:
	jnb	acc.3, t2_enc_4_dec		;2	2
	setb	tracking_check_reqd_4
	inc	r7				;1		rotary4_position = r7
	cjne	r7, #96, t2_enc_4_dec		;2
	mov	r7, #0				;1
t2_enc_4_dec:
	jnb	acc.2, t2_enc_update_prev	;2	2
	setb	tracking_check_reqd_4
	dec	r7
	cjne	r7, #255, t2_enc_update_prev
	mov	r7, #95
t2_enc_update_prev:
	mov	a, b				;1	1
	anl	a, #0x0F			;1	1
	mov	encoder_prev_lsn, a		;1	1
	mov	a, b				;1	1
	swap	a				;1	1
	anl	a, #0x0F			;1	1	38 cycles typical
	mov	encoder_prev_msn, a		;1	1	58 cycles worst case


;--------------------------------;
;  Read Optical Position Sensors ;
;--------------------------------;

t2_optical_1:
	mov	c, optical_1_pin		;1	1
	mov	a, optical_1_history		;1	1
	rlc	a				;1	1
	mov	optical_1_history, a		;1	1
	cjne	a, #0xF0, t2_optical_2		;2	2
t2_optical_1_ccw:
	mov	r4, #3				;1
	mov	r0, #6				;1
	jb	motor1_dir, t2_optical_2	;2
t2_optical_1_cw:
	mov	r4, #92				;1
	mov	r0, #194			;1
t2_optical_2:
	mov	c, optical_2_pin		;1	1
	mov	a, optical_2_history		;1	1
	rlc	a				;1	1
	mov	optical_2_history, a		;1	1
	cjne	a, #0xF0, t2_optical_3		;2	2
t2_optical_2_ccw:
	mov	r5, #3				;1
	mov	r1, #6				;1
	jb	motor2_dir, t2_optical_3	;2
t2_optical_2_cw:
	mov	r5, #92				;1
	mov	r1, #194			;1
t2_optical_3:
	mov	c, optical_3_pin		;1	1
	mov	a, optical_3_history		;1	1
	rlc	a				;1	1
	mov	optical_3_history, a		;1	1
	cjne	a, #0xF0, t2_optical_4		;2	2
t2_optical_3_ccw:
	mov	r6, #3				;1
	mov	r2, #6				;1
	jb	motor3_dir, t2_optical_4	;2
t2_optical_3_cw:
	mov	r6, #92				;1
	mov	r2, #194			;1
t2_optical_4:
	mov	c, optical_4_pin		;1	1
	mov	a, optical_4_history		;1	1
	rlc	a				;1	1
	mov	optical_4_history, a		;1	1
	cjne	a, #0xF0, t2_optical_end	;2	2
t2_optical_4_ccw:
	mov	r7, #3				;1
	mov	r3, #6				;1
	jb	motor4_dir, t2_optical_end	;2
t2_optical_4_cw:
	mov	r7, #92				;1	24 cycles typical
	mov	r3, #194			;1
t2_optical_end:					;	48 cycles worst case


;--------------------------------;
;  Update Motor 1 Position       ;
;--------------------------------;
						;	typ	worst	typ
t2_m1:						;worst	moving	still	still	disable
	jb	motor1_enabled, t2_m1_enabled	;2	2	2	2	2
t2_m1_disabled:
	clr	a				;				1
	mov	dptr, #port_motor1		;				2
	movx	@dptr, a			;				2
	sjmp	t2_m1_done			;				2
t2_m1_enabled:
	mov	a, motor1_speed			;1	1	1
	jnz	t2_m1_moving			;2	2	2
t2_m1_still:
	;if the motor is not moving, make sure it's
	;driven with low current to save power
	mov	a, motor1_step_state		;		1	1
	jb	acc.7, t2_m1_done		;		2	2
	setb	acc.7				;		1
	mov	motor1_step_state, a		;		1
	anl	a, #00000111b			;		1
	mov	dptr, #table_stepper_still	;		2
	movc	a, @a+dptr			;		2
	mov	dptr, #port_motor1		;		2
	movx	@dptr, a			;		2
	sjmp	t2_m1_done			;		2
t2_m1_moving:
	jb	motor1_dir, t2_m1_ccw		;2	2
t2_m1_cw:
	mov	a, motor1_spd_state		;1	1
	add	a, motor1_speed			;1	1
	mov	motor1_spd_state, a		;1	1
	jnc	t2_m1_done			;2	2
	mov	a, motor1_step_state		;1
	inc	a				;1
	anl	a, #00000111b			;1
	mov	motor1_step_state, a		;1
	mov	dptr, #table_stepper_moving	;2
	movc	a, @a+dptr			;2
	mov	dptr, #port_motor1		;2
	movx	@dptr, a			;2
	jnb	motor1_step_state.0, t2_m1_done	;2
	setb	tracking_check_reqd_1
	inc	r0				;1
	cjne	r0, #200, t2_m1_done		;2
	mov	r0, #0				;1
	sjmp	t2_m1_done			;2
t2_m1_ccw:
	mov	a, motor1_spd_state
	clr	c
	subb	a, motor1_speed
	mov	motor1_spd_state, a
	jnc	t2_m1_done
	mov	a, motor1_step_state
	dec	a
	anl	a, #00000111b
	mov	motor1_step_state, a
	mov	dptr, #table_stepper_moving
	movc	a, @a+dptr
	mov	dptr, #port_motor1
	movx	@dptr, a
	jnb	motor1_step_state.0, t2_m1_done	; 9 cycles when disabled
	setb	tracking_check_reqd_1
	dec	r0				; 5 cycles typical when still
	cjne	r0, #255, t2_m1_done		;21 cycles first time still
	mov	r0, #199			;12 cycles typical moving
t2_m1_done:					;32 cycles worst case


;--------------------------------;
;  Update Motor 2 Position       ;
;--------------------------------;

t2_m2:						;worst	moving	still	still	disable
	jb	motor2_enabled, t2_m2_enabled	;2	2	2	2	2
t2_m2_disabled:
	clr	a				;				1
	mov	dptr, #port_motor2		;				2
	movx	@dptr, a			;				2
	sjmp	t2_m2_done			;				2
t2_m2_enabled:
	mov	a, motor2_speed			;1	1	1
	jnz	t2_m2_moving			;2	2	2
t2_m2_still:
	;if the motor is not moving, make sure it's
	;driven with low current to save power
	mov	a, motor2_step_state		;		1	1
	jb	acc.7, t2_m2_done		;		2	2
	setb	acc.7				;		1
	mov	motor2_step_state, a		;		1
	anl	a, #00000111b			;		1
	mov	dptr, #table_stepper_still	;		2
	movc	a, @a+dptr			;		2
	mov	dptr, #port_motor2		;		2
	movx	@dptr, a			;		2
	sjmp	t2_m2_done			;		2
t2_m2_moving:
	jb	motor2_dir, t2_m2_ccw		;2	2
t2_m2_cw:
	mov	a, motor2_spd_state		;1	1
	add	a, motor2_speed			;1	1
	mov	motor2_spd_state, a		;1	1
	jnc	t2_m2_done			;2	2
	mov	a, motor2_step_state		;1
	inc	a				;1
	anl	a, #00000111b			;1
	mov	motor2_step_state, a		;1
	mov	dptr, #table_stepper_moving	;2
	movc	a, @a+dptr			;2
	mov	dptr, #port_motor2		;2
	movx	@dptr, a			;2
	jnb	motor2_step_state.0, t2_m2_done	;2
	setb	tracking_check_reqd_2
	inc	r1				;1
	cjne	r1, #200, t2_m2_done		;2
	mov	r1, #0				;1
	sjmp	t2_m2_done			;2
t2_m2_ccw:
	mov	a, motor2_spd_state
	clr	c
	subb	a, motor2_speed
	mov	motor2_spd_state, a
	jnc	t2_m2_done
	mov	a, motor2_step_state
	dec	a
	anl	a, #00000111b
	mov	motor2_step_state, a
	mov	dptr, #table_stepper_moving
	movc	a, @a+dptr
	mov	dptr, #port_motor2
	movx	@dptr, a
	jnb	motor2_step_state.0, t2_m2_done	; 9 cycles when disabled
	setb	tracking_check_reqd_2
	dec	r1				; 5 cycles typical when still
	cjne	r1, #255, t2_m2_done		;21 cycles first time still
	mov	r1, #199			;12 cycles typical moving
t2_m2_done:					;32 cycles worst case


;--------------------------------;
;  Update Motor 3 Position       ;
;--------------------------------;

t2_m3:						;worst	moving	still	still	disable
	jb	motor3_enabled, t2_m3_enabled	;2	2	2	2	2
t2_m3_disabled:
	clr	a				;				1
	mov	dptr, #port_motor3		;				2
	movx	@dptr, a			;				2
	sjmp	t2_m3_done			;				2
t2_m3_enabled:
	mov	a, motor3_speed			;1	1	1
	jnz	t2_m3_moving			;2	2	2
t2_m3_still:
	;if the motor is not moving, make sure it's
	;driven with low current to save power
	mov	a, motor3_step_state		;		1	1
	jb	acc.7, t2_m3_done		;		2	2
	setb	acc.7				;		1
	mov	motor3_step_state, a		;		1
	anl	a, #00000111b			;		1
	mov	dptr, #table_stepper_still	;		2
	movc	a, @a+dptr			;		2
	mov	dptr, #port_motor3		;		2
	movx	@dptr, a			;		2
	sjmp	t2_m3_done			;		2
t2_m3_moving:
	jb	motor3_dir, t2_m3_ccw		;2	2
t2_m3_cw:
	mov	a, motor3_spd_state		;1	1
	add	a, motor3_speed			;1	1
	mov	motor3_spd_state, a		;1	1
	jnc	t2_m3_done			;2	2
	mov	a, motor3_step_state		;1
	inc	a				;1
	anl	a, #00000111b			;1
	mov	motor3_step_state, a		;1
	mov	dptr, #table_stepper_moving	;2
	movc	a, @a+dptr			;2
	mov	dptr, #port_motor3		;2
	movx	@dptr, a			;2
	jnb	motor3_step_state.0, t2_m3_done	;2
	setb	tracking_check_reqd_3
	inc	r2				;1
	cjne	r2, #200, t2_m3_done		;2
	mov	r2, #0				;1
	sjmp	t2_m3_done			;2
t2_m3_ccw:
	mov	a, motor3_spd_state
	clr	c
	subb	a, motor3_speed
	mov	motor3_spd_state, a
	jnc	t2_m3_done
	mov	a, motor3_step_state
	dec	a
	anl	a, #00000111b
	mov	motor3_step_state, a
	mov	dptr, #table_stepper_moving
	movc	a, @a+dptr
	mov	dptr, #port_motor3
	movx	@dptr, a
	jnb	motor3_step_state.0, t2_m3_done	; 9 cycles when disabled
	setb	tracking_check_reqd_3
	dec	r2				; 5 cycles typical when still
	cjne	r2, #255, t2_m3_done		;21 cycles first time still
	mov	r2, #199			;12 cycles typical moving
t2_m3_done:					;32 cycles worst case


;--------------------------------;
;  Update Motor 4 Position       ;
;--------------------------------;

t2_m4:						;worst	moving	still	still	disable
	jb	motor4_enabled, t2_m4_enabled	;2	2	2	2	2
t2_m4_disabled:
	clr	a				;				1
	mov	dptr, #port_motor4		;				2
	movx	@dptr, a			;				2
	sjmp	t2_m4_done			;				2
t2_m4_enabled:
	mov	a, motor4_speed			;1	1	1
	jnz	t2_m4_moving			;2	2	2
t2_m4_still:
	;if the motor is not moving, make sure it's
	;driven with low current to save power
	mov	a, motor4_step_state		;		1	1
	jb	acc.7, t2_m4_done		;		2	2
	setb	acc.7				;		1
	mov	motor4_step_state, a		;		1
	anl	a, #00000111b			;		1
	mov	dptr, #table_stepper_still	;		2
	movc	a, @a+dptr			;		2
	mov	dptr, #port_motor4		;		2
	movx	@dptr, a			;		2
	sjmp	t2_m4_done			;		2
t2_m4_moving:
	jb	motor4_dir, t2_m4_ccw		;2	2
t2_m4_cw:
	mov	a, motor4_spd_state		;1	1
	add	a, motor4_speed			;1	1
	mov	motor4_spd_state, a		;1	1
	jnc	t2_m4_done			;2	2
	mov	a, motor4_step_state		;1
	inc	a				;1
	anl	a, #00000111b			;1
	mov	motor4_step_state, a		;1
	mov	dptr, #table_stepper_moving	;2
	movc	a, @a+dptr			;2
	mov	dptr, #port_motor4		;2
	movx	@dptr, a			;2
	jnb	motor4_step_state.0, t2_m4_done	;2
	setb	tracking_check_reqd_4
	inc	r3				;1
	cjne	r3, #200, t2_m4_done		;2
	mov	r3, #0				;1
	sjmp	t2_m4_done			;2
t2_m4_ccw:
	mov	a, motor4_spd_state
	clr	c
	subb	a, motor4_speed
	mov	motor4_spd_state, a
	jnc	t2_m4_done
	mov	a, motor4_step_state
	dec	a
	anl	a, #00000111b
	mov	motor4_step_state, a
	mov	dptr, #table_stepper_moving
	movc	a, @a+dptr
	mov	dptr, #port_motor4
	movx	@dptr, a
	jnb	motor4_step_state.0, t2_m4_done	; 9 cycles when disabled
	setb	tracking_check_reqd_4
	dec	r3				; 5 cycles typical when still
	cjne	r3, #255, t2_m4_done		;21 cycles first time still
	mov	r3, #199			;12 cycles typical moving
t2_m4_done:					;32 cycles worst case


;--------------------------------;
;  Delay timers                  ;
;--------------------------------;


t2_delays:
	mov	a, delay1_count			;1
	add	a, #255				;1	decrease count
	subb	a, #255				;1	but don't roll 0->255
	mov	delay1_count, a			;1
	mov	a, delay2_count			;1
	add	a, #255				;1
	subb	a, #255				;1
	mov	delay2_count, a			;1
	mov	a, delay3_count			;1
	add	a, #255				;1
	subb	a, #255				;1
	mov	delay3_count, a			;1
	mov	a, delay4_count			;1
	add	a, #255				;1
	subb	a, #255				;1
	mov	delay4_count, a			;1	16 cycles


;--------------------------------;
;  Exit Interrupt Routine        ;
;--------------------------------;

t2_end:
	pop	dph				;2
	pop	dpl				;2
	pop	b				;2
	pop	acc				;2
	pop	psw				;2
	reti					;2	12 cycles to exit













;*************************************************************************
;**									**
;**	Idle time processing, monitors position tracking between	**
;**	motors and sensors, adjusts motor speeds, works toward		**
;**	making motors reach their target positions			**
;**									**
;*************************************************************************

.org    locat+0x0800

idle:
	mov	a, idle_task_next		;1
	 ;lcall	phex
	inc	a				;1
	cjne	a, #num_idle_tasks, idle_2	;2
	clr	a				;1
idle_2:	mov	idle_task_next, a		;1
	rl	a				;1
	mov	dptr, #idle_task_jump_table	;2
	jmp	@a+dptr				;2
	.equ	num_idle_tasks, 9
idle_task_jump_table:				;2   13 cycles to enter task
	ajmp	motor2_speed_task
	ajmp	motor1_speed_task
	ajmp	motor3_speed_task
	ajmp	motor4_speed_task
	ajmp	motor2_tracking_task
	ajmp	motor1_tracking_task
	ajmp	motor3_tracking_task
	ajmp	motor4_tracking_task
	ajmp	dummy_task

;each task should be designed to run quickly, less than 300 cycles,
;preferably less than 150 cycles if possible.


motor1_speed_task:
	

	ret

motor2_speed_task:
	ret

motor3_speed_task:
	ret

motor4_speed_task:
	ret

;.equ	motor1_position, 0x08		;position (in motor steps), 0 to 199
;.equ	rotary1_position, 0x0C		;position via sensors, 0 to 95


.equ	max_tracking_err, 6


	;check if the motor position is consistent with the position measured
	;by the rotary encoders.  If not, then we'll assume the user is manually
	;turning the wheel and we need to disable this motor
motor1_tracking_task:
	jnb	motor1_enabled, m1_tracking_done	;2
	jnb	tracking_check_reqd_1, m1_tracking_done
	clr	tracking_check_reqd_1
	mov	r2, motor1_position	;2	0 to 199
	mov	a, rotary1_position	;1	0 to 95
	rl	a			;1
	mov	b, #171			;2
	mul	ab			;4
	rlc	a			;1
	mov	a, b			;1
	addc	a, #0			;1
	xch	a, r2			;1	r2 = rotary on 0-127 scale
	mov	b, #164			;2
	mul	ab			;4
	rlc	a			;1
	mov	a, b			;1
	addc	a, #0			;1	;acc = motor on 0-127 scale
	subb	a, r2			;1	compare them to each other (c=0)
	;legal ranges:
	;0x00 to 0x07    0 to 7
	;0xF8 to 0xFF   -1 to -7
	;0x78 to 0x7F  120 to 127
	;0x80 to 0x87 -128 to -121
	;upper 5 bits:
	;0x00	00000xxx
	;0xF8	11111xxx
	;0x78	01111xxx
	;0x80	10000xxx
	mov	c, motor1_dir		;1
	addc	a, #0			;1	add 1 if moving CCW
	mov	c, motor1_dir		;1
	addc	a, #0			;1	add 1 if moving CCW
	mov	c, motor1_dir		;1
	addc	a, #255			;1	subtract 1 if moving CW
	mov	c, motor1_dir		;1
	addc	a, #255			;1	subtract 1 if moving CW
	anl	a, #01111000b
	jz	m1_tracking_ok
	xrl	a, #01111000b
	jz	m1_tracking_ok
	;if we get here, the difference is too much
	inc	tracking_err_count_1
	mov	a, tracking_err_count_1
	cjne	a, #max_tracking_err, m1_tracking_done
	;if we get here, it's been too much for too long
	clr	motor1_enabled
m1_tracking_ok:
	mov	tracking_err_count_1, #0
m1_tracking_done:
	ret				;2


motor2_tracking_task:
	jnb	motor2_enabled, m2_tracking_done	;2
	jnb	tracking_check_reqd_2, m2_tracking_done
	clr	tracking_check_reqd_2
	mov	r2, motor2_position	;2	0 to 199
	mov	a, rotary2_position	;1	0 to 95
	rl	a			;1
	mov	b, #171			;2
	mul	ab			;4
	rlc	a			;1
	mov	a, b			;1
	addc	a, #0			;1
	xch	a, r2			;1	r2 = rotary on 0-127 scale
	mov	b, #164			;2
	mul	ab			;4
	rlc	a			;1
	mov	a, b			;1
	addc	a, #0			;1	;acc = motor on 0-127 scale
	subb	a, r2			;1	compare them to each other (c=0)
	mov	c, motor2_dir		;1
	addc	a, #0			;1	add 1 if moving CCW
	mov	c, motor2_dir		;1
	addc	a, #0			;1	add 1 if moving CCW
	mov	c, motor2_dir		;1
	addc	a, #255			;1	subtract 1 if moving CW
	mov	c, motor2_dir		;1
	addc	a, #255			;1	subtract 1 if moving CW
	anl	a, #01111000b
	jz	m2_tracking_ok
	xrl	a, #01111000b
	jz	m2_tracking_ok
	;if we get here, the difference is too much
	inc	tracking_err_count_2
	mov	a, tracking_err_count_2
	cjne	a, #max_tracking_err, m2_tracking_done
	;if we get here, it's been too much for too long
	clr	motor2_enabled
m2_tracking_ok:
	mov	tracking_err_count_2, #0
m2_tracking_done:
	ret				;2


motor3_tracking_task:
	jnb	motor3_enabled, m3_tracking_done	;2
	jnb	tracking_check_reqd_3, m3_tracking_done
	clr	tracking_check_reqd_3
	mov	r2, motor3_position	;2	0 to 199
	mov	a, rotary3_position	;1	0 to 95
	rl	a			;1
	mov	b, #171			;2
	mul	ab			;4
	rlc	a			;1
	mov	a, b			;1
	addc	a, #0			;1
	xch	a, r2			;1	r2 = rotary on 0-127 scale
	mov	b, #164			;2
	mul	ab			;4
	rlc	a			;1
	mov	a, b			;1
	addc	a, #0			;1	;acc = motor on 0-127 scale
	subb	a, r2			;1	compare them to each other (c=0)
	mov	c, motor3_dir		;1
	addc	a, #0			;1	add 1 if moving CCW
	mov	c, motor3_dir		;1
	addc	a, #0			;1	add 1 if moving CCW
	mov	c, motor3_dir		;1
	addc	a, #255			;1	subtract 1 if moving CW
	mov	c, motor3_dir		;1
	addc	a, #255			;1	subtract 1 if moving CW
	anl	a, #01111000b
	jz	m3_tracking_ok
	xrl	a, #01111000b
	jz	m3_tracking_ok
	;if we get here, the difference is too much
	inc	tracking_err_count_3
	mov	a, tracking_err_count_3
	cjne	a, #max_tracking_err, m3_tracking_done
	;if we get here, it's been too much for too long
	clr	motor3_enabled
m3_tracking_ok:
	mov	tracking_err_count_3, #0
m3_tracking_done:
	ret				;2


motor4_tracking_task:
	jnb	motor4_enabled, m4_tracking_done	;2
	jnb	tracking_check_reqd_4, m4_tracking_done
	clr	tracking_check_reqd_4
	mov	r2, motor4_position	;2	0 to 199
	mov	a, rotary4_position	;1	0 to 95
	rl	a			;1
	mov	b, #171			;2
	mul	ab			;4
	rlc	a			;1
	mov	a, b			;1
	addc	a, #0			;1
	xch	a, r2			;1	r2 = rotary on 0-127 scale
	mov	b, #164			;2
	mul	ab			;4
	rlc	a			;1
	mov	a, b			;1
	addc	a, #0			;1	;acc = motor on 0-127 scale
	subb	a, r2			;1	compare them to each other (c=0)
	mov	c, motor4_dir		;1
	addc	a, #0			;1	add 1 if moving CCW
	mov	c, motor4_dir		;1
	addc	a, #0			;1	add 1 if moving CCW
	mov	c, motor4_dir		;1
	addc	a, #255			;1	subtract 1 if moving CW
	mov	c, motor4_dir		;1
	addc	a, #255			;1	subtract 1 if moving CW
	anl	a, #01111000b
	jz	m4_tracking_ok
	xrl	a, #01111000b
	jz	m4_tracking_ok
	;if we get here, the difference is too much
	inc	tracking_err_count_4
	mov	a, tracking_err_count_4
	cjne	a, #max_tracking_err, m4_tracking_done
	;if we get here, it's been too much for too long
	clr	motor4_enabled
m4_tracking_ok:
	mov	tracking_err_count_4, #0
m4_tracking_done:
	ret				;2



dummy_task:
	ret











;*************************************************************************
;**									**
;**	Initialization - start everything				**
;**									**
;*************************************************************************

.org    locat+0x1000
.db     0xA5,0xE5,0xE0,0xA5     ;signiture bytes
.db     254,'A',0,0              ;id (35=prog)
.db     0,0,0,0                 ;prompt code vector
.db     0,0,0,0                 ;reserved
.db     0,0,0,0                 ;reserved
.db     0,0,0,0                 ;reserved
.db     0,0,0,0                 ;user defined
.db     255,255,255,255         ;length and checksum (255=unused)
.db     "Motor & Sensors",0     ;max 31 characters, plus the zero
.org    locat+0x1000+64         ;executable code begins here



.equ	timer2_interval, 922	;1999 Hz



;Finally, the actual code.... much shorter than all these comments
;that try to explain how to set everything up!
startup:
	jnb	ti, *
	mov	psw, #0
	mov	sp, #stack
	;configure the 82c55 chips
	mov	dptr, #port_abc_pgm
	mov	a, #port_abc_pgm_value
	movx	@dptr, a
	mov	dptr, #port_def_pgm
	mov	a, #port_def_pgm_value
	movx	@dptr, a
	mov	a, #255
	mov	dptr, #port_e
	movx	@dptr, a
	;fill all memory with zeros.  So all variables will be
	;initialized to zero, and we only need to explicitly
	;init the ones that need to be non-zero at startup
	mov	r0, #8
	clr	a
clrloop:mov	@r0, a
	inc	r0
	cjne	r0, #stack, clrloop
	;now initialize non-zero startup values
	mov	a, #0x55
	mov	optical_1_history, a
	mov	optical_2_history, a
	mov	optical_3_history, a
	mov	optical_4_history, a
	;init timer interrupt
	mov	t2con, #0
	mov	rcap2l, #(65536 - timer2_interval) & 255
	mov	rcap2h, #(65536 - timer2_interval) >> 8
	mov	tl2, #(65536 - timer2_interval) & 255
	mov	th2, #(65536 - timer2_interval) >> 8
	mov	ip, #00010000b
	mov	ie, #10100000b	;enable timer2 interrupt
	setb	tr2
	;todo: init uart
	mov	input_buf_ptr, #input_buf
	acall	read_dip_switches
	setb	txen_pin
	clr	rxen_pin



;.equ	motor1_dir, 0			;0 = CW, 1 = CCW


	;setb	motor1_enabled
	;setb	motor1_dir
	mov	motor1_speed, #44

	;setb	motor2_enabled
	setb	motor2_dir
	mov	motor2_speed, #45

	;setb	motor3_enabled
	setb	motor3_dir
	mov	motor3_speed, #46

	;setb	motor4_enabled
	;setb	motor4_dir
	mov	motor4_speed, #47







;*************************************************************************
;**									**
;**	Communication Protocol						**
;**									**
;*************************************************************************


wait_input_loop:
	lcall	idle
	jnb	ri, wait_input_loop
	clr	ri
	mov	a, sbuf		;receive byte from serial port
	 ;lcall	phex
	 ;cjne	a, #27, got_data
	 ;ljmp	0
got_data:
	add	a, #32
	jc	got_control_byte
got_data_byte:
	 ;push	acc
	 ;mov	a, #'.'
	 ;lcall	cout
	 ;pop	acc
	;if we get here, the byte was 0 to 223 (ordinary data), so store
	;it into the buffer and return to waiting for more data.
	add	a, #224
	mov	r0, a
	mov	a, input_buf_ptr
	cjne	a, #input_buf+input_buf_size, store_data_byte
	;input buffer is full, so simply discard this byte
	sjmp	wait_input_loop
store_data_byte:
	xch	a, r0
	mov	@r0, a
	inc	r0
	mov	input_buf_ptr, r0
	sjmp	wait_input_loop

got_control_byte:
	;if we get here, the byte was 224 to 255 (control).  The code
	;above already adjusted it to 0 to 31.  Now we can just use a
	;jump table
	 ;push	acc
	 ;mov	a, #'_'
	 ;lcall	cout
	 ;pop	acc
	mov	dptr, #command_jump_table
	mov	b, #3
	mul	ab
	jmp	@a+dptr
command_jump_table:
	ljmp	cmd_flush_input_buf		;224	begin new data
	ljmp	cmd_flush_input_buf		;225	wheel positions
	ljmp	cmd_flush_input_buf		;226	sonar ping
	ljmp	cmd_flush_input_buf		;227	sonar read request
	ljmp	cmd_flush_input_buf		;228	sonar read reply
	ljmp	cmd_reboot			;229	reboot (to bootloader)
	ljmp	cmd_set_motors			;230	set motors
	ljmp	cmd_read_status			;231	motor status request
	ljmp	cmd_flush_input_buf		;232	motor status reply
	ljmp	cmd_set_led			;233	set led
	ljmp	cmd_flush_input_buf		;234
	ljmp	cmd_flush_input_buf		;235
	ljmp	cmd_flush_input_buf		;236
	ljmp	cmd_flush_input_buf		;237
	ljmp	cmd_flush_input_buf		;238
	ljmp	cmd_flush_input_buf		;239
	ljmp	cmd_flush_input_buf		;240
	ljmp	cmd_flush_input_buf		;241
	ljmp	cmd_flush_input_buf		;242
	ljmp	cmd_flush_input_buf		;243
	ljmp	cmd_flush_input_buf		;244
	ljmp	cmd_flush_input_buf		;245
	ljmp	cmd_flush_input_buf		;246
	ljmp	cmd_flush_input_buf		;247
	ljmp	cmd_flush_input_buf		;248
	ljmp	cmd_flush_input_buf		;249
	ljmp	cmd_flush_input_buf		;250
	ljmp	cmd_flush_input_buf		;251
	ljmp	cmd_flush_input_buf		;252
	ljmp	cmd_flush_input_buf		;253
	ljmp	cmd_flush_input_buf		;254
	ljmp	cmd_flush_input_buf		;255

	nop

cmd_flush_input_buf:
	mov	input_buf_ptr, #input_buf
	acall	read_dip_switches
	ajmp	wait_input_loop


cmd_set_motors:
	mov	a, input_buf_ptr
	cjne	a, #(input_buf + 5), cmd_flush_input_buf	;must be 5 data bytes
	mov	r0, #input_buf
	mov	a, @r0
	add	a, my_address
	jnz	cmd_flush_input_buf
	inc	r0
	mov	a, @r0
	acall	set_motor1
	inc	r0
	mov	a, @r0
	acall	set_motor2
	inc	r0
	mov	a, @r0
	acall	set_motor3
	inc	r0
	mov	a, @r0
	acall	set_motor4
	sjmp	cmd_flush_input_buf


cmd_set_led:
	mov	a, input_buf_ptr
	cjne	a, #(input_buf + 2), cmd_flush_input_buf	;must be 2 data bytes
	mov	r0, #input_buf
	mov	a, @r0
	anl	a, #0x3F
	add	a, my_address
	jnz	cmd_flush_input_buf
	mov	a, @r0
	inc	r0
	mov	c, acc.6
	mov	a, @r0
	mov	acc.7, c
	cpl	a
	mov	dptr, #port_e
	movx	@dptr, a
	sjmp	cmd_flush_input_buf



cmd_reboot:
	mov	a, input_buf_ptr
	cjne	a, #(input_buf + 3), cmd_flush_input_buf
	mov	r0, #input_buf
	cjne	@r0, #195, cmd_flush_input_buf	;data must be 195, 85, 19
	inc	r0
	cjne	@r0, #85, cmd_flush_input_buf
	inc	r0
	cjne	@r0, #19, cmd_flush_input_buf
	;ok, that's it.  We're supposed to jump to the bootloader, and
	;presumably new firmware is about to be downloaded.
	mov	ie, #0
	ljmp	0xF040




cmd_read_status:
	mov	a, input_buf_ptr
	cjne	a, #(input_buf + 1), cmd_flush_input_buf
	mov	r0, #input_buf
	mov	a, @r0
	add	a, my_address
	jnz	cmd_flush_input_buf
	mov	a, flags1
	swap	a
	anl	a, #0x0F		;low 4 bits are status
	mov	r0, a
	clr	a			
	clr	c
	subb	a, my_address
	swap	a
	anl	a, #0x70		;bits 4,5,6 are addr check
	orl	a, r0
	mov	r0, #input_buf
	mov	@r0, a			;byte 1: enabled bits + addr check
	inc	r0
	mov	@r0, rotary1_position	;byte 2: position #1
	add	a, @r0
	inc	r0
	mov	@r0, rotary2_position	;byte 3: position #2
	add	a, @r0
	inc	r0
	mov	@r0, rotary3_position	;byte 4: position #3
	add	a, @r0
	inc	r0
	mov	@r0, rotary4_position	;byte 5: position #4
	add	a, @r0
	inc	r0
	anl	a, #0x7F
	mov	@r0, a			;byte 6: checksum
	inc	r0
	mov	@r0, #232
	inc	r0
	mov	input_buf_ptr, r0
	ajmp	transmit_response




read_dip_switches:
	;mov	my_address, #256 - 5
	clr     a
dipsw1: jb      p1.6, dipsw2
	setb    acc.3
dipsw2: jb      p1.7, dipsw3
	setb    acc.2
dipsw3: jb      p3.2, dipsw4
	setb    acc.1
dipsw4: jb      p3.3, dipsw_end
	setb    acc.0
dipsw_end:
	cpl	a
	inc	a
	mov	my_address, a
	ret

	

	;transmit a response on the RS485 network.  The complete response
	;must be in "input_buffer" and "input_buf_ptr" must point to the
	;byte after the end of the response.
transmit_response:
	;first, delay 2 character times before turning on the transmitter,
	;to allow time for the controller that send up a request to turn
	;off its transmitter.
	acall	byte_delay
	acall	byte_delay
	;now let's transmit
	setb	rxen_pin
	clr	txen_pin
	mov	r0, #input_buf		;r0 point to buffer data
	mov	a, input_buf_ptr
	add	a, #(256 - input_buf)
	mov	r2, a			;r2 is number of bytes in buffer
transmit_response_loop:
	jnb	ti, transmit_response_loop
	clr	ti
	mov	sbuf, @r0
	 ;mov	a, @r0
	 ;lcall	phex
	inc	r0
	djnz	r2, transmit_response_loop
	 ;lcall	newline
	setb	es
transmit_wait_done:
	jnb	ti, transmit_wait_done
transmit_response_end:
	;setb	txen_pin	;interrupt does this for us
	;clr	rxen_pin
	ajmp	cmd_flush_input_buf






	;this code delays exactly one byte time (assuming no interrupts)
	;TODO: change this to use timer0, so we have less chance of
	;inaccurate delay due to heavy use of periodic interrupts
byte_delay:			;2
	mov	a, th1		;1  read baud const
	cpl	a		;1
	inc	a		;1  a = # cycles per 1/16 bit
	mov	b, #20		;1
	mul	ab		;4  a/b has number of cycles / 8
	add	a, #254		;1
	xch	a, b		;1  reduce by 2 for the 16 cycles of
	addc	a, #255		;1  to compute all this, and the ret
	xch	a, b		;1
byte_delay_loop:
	nop			;1  now delay exactly 8 cycles for
	nop			;1  each count in a/b
	add	a, #255		;1
	xch	a, b		;1
	addc	a, #255		;1
	xch	a, b		;1
	jc	byte_delay_loop	;2
	ret			;2



	;acc = new setting
	; 0 to 63 = CW  (0 = stop, 1 = slow, 63 = fast)
	; 64 to 127 = CCW  (64 = stop, 65 = slow, 127 = fast)
	; 128 to 197 = undefined
	; 198 = disable motor
	; 199 = no change
set_motor1:
	cjne	a, #198, set_m1
	clr	motor1_enabled
	ret
set_m1:
	jnb	acc.7, set_m1_ok
	ret
set_m1_ok:
	mov	c, acc.6
	anl	a, #00111111b
	clr	ea
	mov	motor1_speed, a
	mov	motor1_dir, c
	anl	motor1_step_state, #01111111b
	setb	ea
	jb	motor1_enabled, set_m1_done
	mov	a, rotary1_position	;0 to 95 scale
	acall	rotary_to_motor_scale
	mov	motor1_position, a
	mov	tracking_err_count_1, #0
	setb	motor1_enabled
set_m1_done:
	ret


set_motor2:
	cjne	a, #198, set_m2
	clr	motor2_enabled
	ret
set_m2:
	jnb	acc.7, set_m2_ok
	ret
set_m2_ok:
	mov	c, acc.6
	anl	a, #00111111b
	clr	ea
	mov	motor2_speed, a
	mov	motor2_dir, c
	anl	motor2_step_state, #01111111b
	setb	ea
	jb	motor2_enabled, set_m2_done
	mov	a, rotary2_position	;0 to 95 scale
	acall	rotary_to_motor_scale
	mov	motor2_position, a
	mov	tracking_err_count_2, #0
	setb	motor2_enabled
set_m2_done:
	ret


set_motor3:
	cjne	a, #198, set_m3
	clr	motor3_enabled
	ret
set_m3:
	jnb	acc.7, set_m3_ok
	ret
set_m3_ok:
	mov	c, acc.6
	anl	a, #00111111b
	clr	ea
	mov	motor3_speed, a
	mov	motor3_dir, c
	anl	motor3_step_state, #01111111b
	setb	ea
	jb	motor3_enabled, set_m3_done
	mov	a, rotary3_position	;0 to 95 scale
	acall	rotary_to_motor_scale
	mov	motor3_position, a
	mov	tracking_err_count_3, #0
	setb	motor3_enabled
set_m3_done:
	ret


set_motor4:
	cjne	a, #198, set_m4
	clr	motor4_enabled
	ret
set_m4:
	jnb	acc.7, set_m4_ok
	ret
set_m4_ok:
	mov	c, acc.6
	anl	a, #00111111b
	clr	ea
	mov	motor4_speed, a
	mov	motor4_dir, c
	anl	motor4_step_state, #01111111b
	setb	ea
	jb	motor4_enabled, set_m4_done
	mov	a, rotary4_position	;0 to 95 scale
	acall	rotary_to_motor_scale
	mov	motor4_position, a
	mov	tracking_err_count_4, #0
	setb	motor4_enabled
set_m4_done:
	ret





;*************************************************************************
;**									**
;**	Lookup tables for rapid calculations				**
;**									**
;*************************************************************************


	;acc input 0 to 95 range, output 0 to 199 range
rotary_to_motor_scale:
	inc	a
	movc	a, @a+pc
	ret
	.db	0	; 0 -> 0.00
	.db	2	; 1 -> 2.08
	.db	4	; 2 -> 4.17
	.db	6	; 3 -> 6.25
	.db	8	; 4 -> 8.33
	.db	10	; 5 -> 10.42
	.db	13	; 6 -> 12.50
	.db	15	; 7 -> 14.58
	.db	17	; 8 -> 16.67
	.db	19	; 9 -> 18.75
	.db	21	; 10 -> 20.83
	.db	23	; 11 -> 22.92
	.db	25	; 12 -> 25.00
	.db	27	; 13 -> 27.08
	.db	29	; 14 -> 29.17
	.db	31	; 15 -> 31.25
	.db	33	; 16 -> 33.33
	.db	35	; 17 -> 35.42
	.db	38	; 18 -> 37.50
	.db	40	; 19 -> 39.58
	.db	42	; 20 -> 41.67
	.db	44	; 21 -> 43.75
	.db	46	; 22 -> 45.83
	.db	48	; 23 -> 47.92
	.db	50	; 24 -> 50.00
	.db	52	; 25 -> 52.08
	.db	54	; 26 -> 54.17
	.db	56	; 27 -> 56.25
	.db	58	; 28 -> 58.33
	.db	60	; 29 -> 60.42
	.db	63	; 30 -> 62.50
	.db	65	; 31 -> 64.58
	.db	67	; 32 -> 66.67
	.db	69	; 33 -> 68.75
	.db	71	; 34 -> 70.83
	.db	73	; 35 -> 72.92
	.db	75	; 36 -> 75.00
	.db	77	; 37 -> 77.08
	.db	79	; 38 -> 79.17
	.db	81	; 39 -> 81.25
	.db	83	; 40 -> 83.33
	.db	85	; 41 -> 85.42
	.db	88	; 42 -> 87.50
	.db	90	; 43 -> 89.58
	.db	92	; 44 -> 91.67
	.db	94	; 45 -> 93.75
	.db	96	; 46 -> 95.83
	.db	98	; 47 -> 97.92
	.db	100	; 48 -> 100.00
	.db	102	; 49 -> 102.08
	.db	104	; 50 -> 104.17
	.db	106	; 51 -> 106.25
	.db	108	; 52 -> 108.33
	.db	110	; 53 -> 110.42
	.db	113	; 54 -> 112.50
	.db	115	; 55 -> 114.58
	.db	117	; 56 -> 116.67
	.db	119	; 57 -> 118.75
	.db	121	; 58 -> 120.83
	.db	123	; 59 -> 122.92
	.db	125	; 60 -> 125.00
	.db	127	; 61 -> 127.08
	.db	129	; 62 -> 129.17
	.db	131	; 63 -> 131.25
	.db	133	; 64 -> 133.33
	.db	135	; 65 -> 135.42
	.db	138	; 66 -> 137.50
	.db	140	; 67 -> 139.58
	.db	142	; 68 -> 141.67
	.db	144	; 69 -> 143.75
	.db	146	; 70 -> 145.83
	.db	148	; 71 -> 147.92
	.db	150	; 72 -> 150.00
	.db	152	; 73 -> 152.08
	.db	154	; 74 -> 154.17
	.db	156	; 75 -> 156.25
	.db	158	; 76 -> 158.33
	.db	160	; 77 -> 160.42
	.db	163	; 78 -> 162.50
	.db	165	; 79 -> 164.58
	.db	167	; 80 -> 166.67
	.db	169	; 81 -> 168.75
	.db	171	; 82 -> 170.83
	.db	173	; 83 -> 172.92
	.db	175	; 84 -> 175.00
	.db	177	; 85 -> 177.08
	.db	179	; 86 -> 179.17
	.db	181	; 87 -> 181.25
	.db	183	; 88 -> 183.33
	.db	185	; 89 -> 185.42
	.db	188	; 90 -> 187.50
	.db	190	; 91 -> 189.58
	.db	192	; 92 -> 191.67
	.db	194	; 93 -> 193.75
	.db	196	; 94 -> 195.83
	.db	198	; 95 -> 197.92




table_stepper_moving:
	;	aabbcc dd
	;	fhfhfh fh
	;	765432 10
	.db	10000000b	;1  a full power
	.db	01010000b	;2  a + b half power
	.db	00100000b	;3  b full power
	.db	00010100b	;4  b + c half power
	.db	00001000b	;5  c full power
	.db	00000101b	;6  c + d half power
	.db	00000010b	;7  d full power
	.db	01000001b	;8  d + a half power

table_stepper_still:
	.db	01000000b	;1  a half power
	.db	01000000b	;2  a half power
	.db	00010000b	;3  b half power
	.db	00010000b	;4  b half power
	.db	00000100b	;5  c half power
	.db	00000100b	;6  c half power
	.db	00000001b	;7  d half power
	.db	00000001b	;8  d half power


table_encoder_change:
	.db	0x0		; 0000 0000
	.db	0x2		; 0000 0001
	.db	0x1		; 0000 0010
	.db	0x0		; 0000 0011
	.db	0x8		; 0000 0100
	.db	0xA		; 0000 0101
	.db	0x9		; 0000 0110
	.db	0x8		; 0000 0111
	.db	0x4		; 0000 1000
	.db	0x6		; 0000 1001
	.db	0x5		; 0000 1010
	.db	0x4		; 0000 1011
	.db	0x0		; 0000 1100
	.db	0x2		; 0000 1101
	.db	0x1		; 0000 1110
	.db	0x0		; 0000 1111
	.db	0x1		; 0001 0000
	.db	0x0		; 0001 0001
	.db	0x0		; 0001 0010
	.db	0x2		; 0001 0011
	.db	0x9		; 0001 0100
	.db	0x8		; 0001 0101
	.db	0x8		; 0001 0110
	.db	0xA		; 0001 0111
	.db	0x5		; 0001 1000
	.db	0x4		; 0001 1001
	.db	0x4		; 0001 1010
	.db	0x6		; 0001 1011
	.db	0x1		; 0001 1100
	.db	0x0		; 0001 1101
	.db	0x0		; 0001 1110
	.db	0x2		; 0001 1111
	.db	0x2		; 0010 0000
	.db	0x0		; 0010 0001
	.db	0x0		; 0010 0010
	.db	0x1		; 0010 0011
	.db	0xA		; 0010 0100
	.db	0x8		; 0010 0101
	.db	0x8		; 0010 0110
	.db	0x9		; 0010 0111
	.db	0x6		; 0010 1000
	.db	0x4		; 0010 1001
	.db	0x4		; 0010 1010
	.db	0x5		; 0010 1011
	.db	0x2		; 0010 1100
	.db	0x0		; 0010 1101
	.db	0x0		; 0010 1110
	.db	0x1		; 0010 1111
	.db	0x0		; 0011 0000
	.db	0x1		; 0011 0001
	.db	0x2		; 0011 0010
	.db	0x0		; 0011 0011
	.db	0x8		; 0011 0100
	.db	0x9		; 0011 0101
	.db	0xA		; 0011 0110
	.db	0x8		; 0011 0111
	.db	0x4		; 0011 1000
	.db	0x5		; 0011 1001
	.db	0x6		; 0011 1010
	.db	0x4		; 0011 1011
	.db	0x0		; 0011 1100
	.db	0x1		; 0011 1101
	.db	0x2		; 0011 1110
	.db	0x0		; 0011 1111
	.db	0x4		; 0100 0000
	.db	0x6		; 0100 0001
	.db	0x5		; 0100 0010
	.db	0x4		; 0100 0011
	.db	0x0		; 0100 0100
	.db	0x2		; 0100 0101
	.db	0x1		; 0100 0110
	.db	0x0		; 0100 0111
	.db	0x0		; 0100 1000
	.db	0x2		; 0100 1001
	.db	0x1		; 0100 1010
	.db	0x0		; 0100 1011
	.db	0x8		; 0100 1100
	.db	0xA		; 0100 1101
	.db	0x9		; 0100 1110
	.db	0x8		; 0100 1111
	.db	0x5		; 0101 0000
	.db	0x4		; 0101 0001
	.db	0x4		; 0101 0010
	.db	0x6		; 0101 0011
	.db	0x1		; 0101 0100
	.db	0x0		; 0101 0101
	.db	0x0		; 0101 0110
	.db	0x2		; 0101 0111
	.db	0x1		; 0101 1000
	.db	0x0		; 0101 1001
	.db	0x0		; 0101 1010
	.db	0x2		; 0101 1011
	.db	0x9		; 0101 1100
	.db	0x8		; 0101 1101
	.db	0x8		; 0101 1110
	.db	0xA		; 0101 1111
	.db	0x6		; 0110 0000
	.db	0x4		; 0110 0001
	.db	0x4		; 0110 0010
	.db	0x5		; 0110 0011
	.db	0x2		; 0110 0100
	.db	0x0		; 0110 0101
	.db	0x0		; 0110 0110
	.db	0x1		; 0110 0111
	.db	0x2		; 0110 1000
	.db	0x0		; 0110 1001
	.db	0x0		; 0110 1010
	.db	0x1		; 0110 1011
	.db	0xA		; 0110 1100
	.db	0x8		; 0110 1101
	.db	0x8		; 0110 1110
	.db	0x9		; 0110 1111
	.db	0x4		; 0111 0000
	.db	0x5		; 0111 0001
	.db	0x6		; 0111 0010
	.db	0x4		; 0111 0011
	.db	0x0		; 0111 0100
	.db	0x1		; 0111 0101
	.db	0x2		; 0111 0110
	.db	0x0		; 0111 0111
	.db	0x0		; 0111 1000
	.db	0x1		; 0111 1001
	.db	0x2		; 0111 1010
	.db	0x0		; 0111 1011
	.db	0x8		; 0111 1100
	.db	0x9		; 0111 1101
	.db	0xA		; 0111 1110
	.db	0x8		; 0111 1111
	.db	0x8		; 1000 0000
	.db	0xA		; 1000 0001
	.db	0x9		; 1000 0010
	.db	0x8		; 1000 0011
	.db	0x0		; 1000 0100
	.db	0x2		; 1000 0101
	.db	0x1		; 1000 0110
	.db	0x0		; 1000 0111
	.db	0x0		; 1000 1000
	.db	0x2		; 1000 1001
	.db	0x1		; 1000 1010
	.db	0x0		; 1000 1011
	.db	0x4		; 1000 1100
	.db	0x6		; 1000 1101
	.db	0x5		; 1000 1110
	.db	0x4		; 1000 1111
	.db	0x9		; 1001 0000
	.db	0x8		; 1001 0001
	.db	0x8		; 1001 0010
	.db	0xA		; 1001 0011
	.db	0x1		; 1001 0100
	.db	0x0		; 1001 0101
	.db	0x0		; 1001 0110
	.db	0x2		; 1001 0111
	.db	0x1		; 1001 1000
	.db	0x0		; 1001 1001
	.db	0x0		; 1001 1010
	.db	0x2		; 1001 1011
	.db	0x5		; 1001 1100
	.db	0x4		; 1001 1101
	.db	0x4		; 1001 1110
	.db	0x6		; 1001 1111
	.db	0xA		; 1010 0000
	.db	0x8		; 1010 0001
	.db	0x8		; 1010 0010
	.db	0x9		; 1010 0011
	.db	0x2		; 1010 0100
	.db	0x0		; 1010 0101
	.db	0x0		; 1010 0110
	.db	0x1		; 1010 0111
	.db	0x2		; 1010 1000
	.db	0x0		; 1010 1001
	.db	0x0		; 1010 1010
	.db	0x1		; 1010 1011
	.db	0x6		; 1010 1100
	.db	0x4		; 1010 1101
	.db	0x4		; 1010 1110
	.db	0x5		; 1010 1111
	.db	0x8		; 1011 0000
	.db	0x9		; 1011 0001
	.db	0xA		; 1011 0010
	.db	0x8		; 1011 0011
	.db	0x0		; 1011 0100
	.db	0x1		; 1011 0101
	.db	0x2		; 1011 0110
	.db	0x0		; 1011 0111
	.db	0x0		; 1011 1000
	.db	0x1		; 1011 1001
	.db	0x2		; 1011 1010
	.db	0x0		; 1011 1011
	.db	0x4		; 1011 1100
	.db	0x5		; 1011 1101
	.db	0x6		; 1011 1110
	.db	0x4		; 1011 1111
	.db	0x0		; 1100 0000
	.db	0x2		; 1100 0001
	.db	0x1		; 1100 0010
	.db	0x0		; 1100 0011
	.db	0x4		; 1100 0100
	.db	0x6		; 1100 0101
	.db	0x5		; 1100 0110
	.db	0x4		; 1100 0111
	.db	0x8		; 1100 1000
	.db	0xA		; 1100 1001
	.db	0x9		; 1100 1010
	.db	0x8		; 1100 1011
	.db	0x0		; 1100 1100
	.db	0x2		; 1100 1101
	.db	0x1		; 1100 1110
	.db	0x0		; 1100 1111
	.db	0x1		; 1101 0000
	.db	0x0		; 1101 0001
	.db	0x0		; 1101 0010
	.db	0x2		; 1101 0011
	.db	0x5		; 1101 0100
	.db	0x4		; 1101 0101
	.db	0x4		; 1101 0110
	.db	0x6		; 1101 0111
	.db	0x9		; 1101 1000
	.db	0x8		; 1101 1001
	.db	0x8		; 1101 1010
	.db	0xA		; 1101 1011
	.db	0x1		; 1101 1100
	.db	0x0		; 1101 1101
	.db	0x0		; 1101 1110
	.db	0x2		; 1101 1111
	.db	0x2		; 1110 0000
	.db	0x0		; 1110 0001
	.db	0x0		; 1110 0010
	.db	0x1		; 1110 0011
	.db	0x6		; 1110 0100
	.db	0x4		; 1110 0101
	.db	0x4		; 1110 0110
	.db	0x5		; 1110 0111
	.db	0xA		; 1110 1000
	.db	0x8		; 1110 1001
	.db	0x8		; 1110 1010
	.db	0x9		; 1110 1011
	.db	0x2		; 1110 1100
	.db	0x0		; 1110 1101
	.db	0x0		; 1110 1110
	.db	0x1		; 1110 1111
	.db	0x0		; 1111 0000
	.db	0x1		; 1111 0001
	.db	0x2		; 1111 0010
	.db	0x0		; 1111 0011
	.db	0x4		; 1111 0100
	.db	0x5		; 1111 0101
	.db	0x6		; 1111 0110
	.db	0x4		; 1111 0111
	.db	0x8		; 1111 1000
	.db	0x9		; 1111 1001
	.db	0xA		; 1111 1010
	.db	0x8		; 1111 1011
	.db	0x0		; 1111 1100
	.db	0x1		; 1111 1101
	.db	0x2		; 1111 1110
	.db	0x0		; 1111 1111