;SIC PROJECT 056 LOW-COST PCB MILLING MACHINE
;INITIAL VERSION FOR MIT COURSE 2 BACHELOR'S THESIS "Rapid Prototyping of Rapid Prototyping Machines" SUPERVISED BY NEIL GERSHENFELD, DIRECTOR, MIT CBA

;--REVISION HISTORY-----------------------------------------------------------------------
;
;  ---------------------------------------------------------------------------------
;  | DATE	| MODIFICATIONS				| NAME				|FILENAME				|
;  |--------|---------------------------|-------------------|-----------------------|
;  |03/09/08| CREATED					|ILAN E MOYER		|i0.smc.03.asm			|
;  |--------|---------------------------|-------------------|-----------------------|
;  |12/31/08| SPIFFIFIED				|ILAN E MOYER		|056-054.asm			|
;  |--------|---------------------------|-------------------|-----------------------|
;  |3/20/09 | ADDED HALF-STEPPING		|ILAN E MOYER		|056-054b.asm			|		I changed the format of motorpositions to allow 8 half-positions
;  |--------|---------------------------|-------------------|-----------------------|
;  |3/20/09 | FIXED SINGLE-STEP ISSUE   |ILAN E MOYER		|056-054c.asm			|
;  |--------|---------------------------|-------------------|-----------------------|
;  |11/29/09| MODIFIED FOR NETWORKING   |ILAN E MOYER       |056-054d.asm           |
;  |--------|---------------------------|-------------------|-----------------------|
;  |12/08/09| ADDED VM URL RESPONSE    	|ILAN E MOYER  		|056-054e.asm			|
;  |--------|---------------------------|-------------------|-----------------------|
;  |12/13/09| ADDED 3-AXIS MOTION       |ILAN E MOYER       |056-054f.asm           |
;  |--------|---------------------------|-------------------|-----------------------|
;  |01/06/10| REFACTORED FOR FABINABOX  |ILAN E MOYER       |076-001a.asm           |
;  |--------|---------------------------|-------------------|-----------------------|
;  |01/07/10| ADDED RX PULLUP, LEDS OFF |ILAN E MOYER       |076-001a.asm           | RX pullup is necessary to avoid tripping rcv watchdog. Also turned off ind leds after move.
;  |--------|---------------------------|-------------------|-----------------------|
;  |01/10/10| FIXED MOTION CODE		    |ILAN E MOYER       |076-001b.asm           |
;  |--------|---------------------------|-------------------|-----------------------|
;  |01/12/10| ADDED STATUS RESPONSE		|ILAN E MOYER       |076-001c.asm           | Also fixed push/pop temp3 and temp4 in motion interrupt.
;  |--------|---------------------------|-------------------|-----------------------|
;  |01/16/10| ADDED BACKLASH COMP.		|ILAN E MOYER       |076-001c.asm           | Also added SEI/CLI when accessing 16 bit registers in main loop
;  |--------|---------------------------|-------------------|-----------------------|
;  |02/19/10| NOW SERVO CONTROL CODE	|ILAN E MOYER       |076-007a.asm           |
;  ---------------------------------------------------------------------------------- 
;
;
;
;


;--HARDWARE CONFIGURATION (076-007 REV1)---------------------------------------------
;                       
;
;
;								D C
;								
;								I I
;								N N
;								D D
;								I I
;					T			C C
;					X			A A
;								T T
;					E	  _		O O
;					N	  R		R R
;				    A     E
;				    B T R S P P L L
;				    L X X E C C E E
;					E D D T 5 4 D D
;                  	--------------- 
;	            LED|			   | B INDICATOR LED
;			 BUTTON|			   | A INDICATOR LED
;   	        GND|               | ADC7
;	            VCC|	ATMEGA88   | GND
;               GND|			   | AREF
;               VCC|			   | ADC6
;             XTAL1|			   | AVCC
;             XTAL2|			   | SCK/SERVO 8
;	                ---------------
;					S S S S S S M M
;					E E E E E E O I
;					R R R R R R S S
;					V V V V V V I O
;					O O O O O O /
;					            S
;					1 2 3 4 5 6 E            
;					            R
;					            V
;					            O
;					     
;					   			7
;				     
;
;


;--PORT B ASSIGNMENTS--------------------------------------------------------------
;
;	PB0 = SERVO 4
;   PB1 = SERVO 5
;   PB2 = SERVO 6
;   PB3 = SERVO 7
;   PB4 = MISO
;	PB5 = SCK /  SERVO 8
;	PB6 = XTAL1
;   PB7 = XTAL2
;



;--PORT C ASSIGNMENTS--------------------------------------------------------------
;
;   PC0 = A INDICATOR LED
;   PC1 = B INDICATOR LED
;   PC2 = C INDICATOR LED
;   PC3 = D INDICATOR LED
;   PC4 = N/C
;   PC5	= N/C
;   PC6 = /RESET



;--PORT D ASSIGNMENTS--------------------------------------------------------------
;	PD0 = RXD
;   PD1 = TXD
;   PD2 = RS485 TX ENABLE
;   PD3 = LED
;   PD4 = BUTTON
;	PD5 = SERVO 1
;	PD6 = SERVO 2
;   PD7 = SERVO 3


;--INCLUDE FILES-------------------------------------------------------------------
;    .DEVICE ATMega88

.include "m88def.inc"

;--DEFINITIONS---------------------------------------------------------------------


;PORT DIRECTION MASKS


;PACKET LOCATIONS

	.equ rx_packet_location	 =	0x0100		;SRAM start location for rx packet
	.equ tx_packet_location  =	0x0200		;SRAM start location for tx packet
	
;PAcKET OFFSETS
	.equ packet_startbyte		=	0
	.equ packet_srcaddress0  	=	1
	.equ packet_srcaddress1		=	2
	.equ packet_srcport			=	3
	.equ packet_destaddress0	=	4
	.equ packet_destaddress1	=	5
	.equ packet_destport		=	6
	.equ packet_length			=	7
	.equ packet_payload			=	8
	
;PACKET STANDARD VALUES
	.equ packet_unicast_startbyte = 	74
	.equ packet_broadcast_startbyte = 	138

	
;NODE OFFSETS
	.equ node_address_location = 0x0300		;SRAM start location for node address
	
	

;NODE VALUES
	.equ node_address0 = 165
	.equ node_address1 = 134

;RECEIVER WATCHDOG CONSTANTS
	.equ watchdog_trip = 253


;PORTS: UI
	.equ button_input	=	PIND
	.equ led_output		=	PORTD
	.equ led_toggle		=	PIND
	
;PORTS: COMM
	.equ comm_port		=	PORTD
	.equ comm_dir		=	DDRD
	
;PORTS: SERVO CONTROL

	.equ servo1_ddr			= DDRD
	.equ servo1_port		= PORTD
	
	.equ servo2_ddr			= DDRD
	.equ servo2_port		= PORTD
	
	.equ servo3_ddr			= DDRD
	.equ servo3_port		= PORTD
	
	.equ servo4_ddr			= DDRB
	.equ servo4_port		= PORTB
	
	.equ servo5_ddr			= DDRB
	.equ servo5_port		= PORTB
	
	.equ servo6_ddr			= DDRB
	.equ servo6_port		= PORTB
	
	.equ servo7_ddr			= DDRB
	.equ servo7_port		= PORTB
	
	.equ servo8_ddr			= DDRB
	.equ servo8_port		= PORTB
	
	
;PORTS: INDICATOR LEDS

	.equ indicator_ddr		= DDRC
	.equ indicator_port		= PORTC
	.equ indicator_toggle	= PINC
	
	
	
;PINS: UI
	.equ button			=	PD4
	.equ led			=	PD3
	
;PINS: COMM
	.equ rx_pin			= PD0
	.equ tx_pin			= PD1
	.equ tx_enable		= PD2
	
;PINS: SERVO CONTROL

	.equ servo1		= PD5
	.equ servo2		= PD6
	.equ servo3		= PD7
	.equ servo4		= PB0
	.equ servo5		= PB1
	.equ servo6		= PB2
	.equ servo7		= PB3
	.equ servo8		= PB4
	
;PINS: INDICATOR LED
	.equ ind_a		= PC0
	.equ ind_b		= PC1
	.equ ind_c		= PC2
	.equ ind_d		= PC3
	
	
;DIRECTION MASKS
	.equ portb_directions = (1 << servo4)|(1 << servo5)|(1 << servo6)|(1 << servo7) | (1 << servo8)
	.equ portc_directions = (1 << ind_a)|(1 << ind_b)|(1 << ind_c)|(1<<ind_d)
	.equ portd_directions = (1 << tx_pin)|(1 << tx_enable)|(1  << led)|(1 << servo1)|(1 << servo2)|(1 << servo3)
	
;PULLUP MASKS
	.equ portb_pullups = 0
	.equ portc_pullups = 0
	.equ portd_pullups = (1 << button)|(1 << rx_pin)


;REGISTERS
	.def numzero			= r0
	.def current_step_0		= r1
	.def packetlength		= r2
	.def packetchecksum		= r3
	.def rcv_watchdog		= r4
	



; working registers
	
    .def temp1				=	r16
    .def temp2				=	r17
	.def temp3				=	r18
	.def temp4				= 	r19
	.def packetposition		= 	r20
	.def servo1_byte0		=	r21
	.def servo1_byte1		=	r22




; FLAG REGISTERS

	.equ comm_status		=	GPIOR0			;using general purpose IO register to conserve standard registers.
	.equ packet_received	=	0				;first bit of comm_status.
	.equ packet_inbound		=	1
	.equ packet_outbound	=	2
	
	.equ intermission		=	3				;indicates that in between position pulses.

	
	

;SRAM

    .equ Memory_Start	= 0x0060

	

;--PROGRAM INITIALIZATION----------------------------------------------------------

.cseg
.org 0

;--INTERRUPT VECTOR TABLE----------------------------------------------------------

    rjmp    reset                           ; External Pin, Power-on Reset, Brown-out Reset and Watchdog System
    .dw     0                               ; External Interrupt Request 0 
    .dw     0                               ; External Interrupt Request 1
    .dw     0                               ; Pin Change Interrupt Request 0
    .dw     0                               ; Pin Change Interrupt Request 1
    .dw     0	                            ; Pin Change Interrupt Request 2
    .dw     0	 			               	; Watchdog Time-out Interrupt
    .dw		0			                    ; Timer/Counter2 Compare Match A
    .dw     0		                    	; Timer/Counter2 Compare Match B
    rjmp    isr_packet_timeout_100          ; Timer/Counter2 Overflow
    .dw     0                               ; Timer/Counter1 Capture Event
    .dw		0				        		; Timer/Counter1 Compare Match A
	.dw		0     							; Timer/Coutner1 Compare Match B
    rjmp    isr_servo_100                   ; Timer/Counter1 Overflow
    .dw     0			                    ; Timer/Counter0 Compare Match A
    .dw     0                               ; Timer/Counter0 Compare Match B
    .dw     0					            ; Timer/Counter0 Overflow
	.dw		0								; SPI Serial Transfer Complete
	rjmp	isr_packet_receiver_100			; USART Rx Complete
	rjmp	isr_packet_transmitter_100		; USART, Data Register Empty
	rjmp	isr_packet_transmitter_100		; USART, Tx Complete
	.dw		0								; ADC Conversion Complete
	.dw		0								; EEPROM Ready
	.dw		0								; Analog Comparator
	.dw		0								; 2-wire Serial Interface
	.dw		0								; Store Program Memory Ready



	

;--INTERRUPT ROUTINES--------------------------------------------------------------

;-- PACKET RECEIVER --
;
;This interrupt routine stores data from an incoming packet into SRAM
;


isr_packet_receiver_100:
	cli
	push ZL
	push ZH
	push temp1
	push temp2
	in temp2, SREG
	push temp2
	
	sts TCNT2, numzero		;clears timer0
	mov rcv_watchdog, numzero	;resets watchdog counter
    ldi     temp1, (0 << OCIE0B)|(0 << OCIE0A)|(1 << TOIE0)		;enables timer2 as watchdog for receiver	
    sts     TIMSK2, TEMP1
	
	
	
isr_packet_receiver_200:	;load byte into temp1, and store in SRAM
	lds temp1, UDR0
	
	ldi ZH, high(rx_packet_location)
	ldi ZL, low(rx_packet_location)
	add ZL, packetposition
	adc ZH, numzero
	st Z, temp1
	
	sbi comm_status, packet_inbound		;sets packet inbound flag
	
	
isr_packet_receiver_300:	;check for packet length

	cpi packetposition, packet_length	;see if this byte contains the packet length
	brne isr_packet_receiver_400
	mov packetlength, temp1				;if YES, store the packet length in packetlength register


isr_packet_receiver_400:	;checks for end-of-packet condition
	cpi packetposition, packet_length	
	brlo isr_packet_receiver_500		;checks if the packet length byte has been read yet. If YES, checks for end-of-packet
	cp packetposition, packetlength
	breq isr_packet_receiver_600		;if this is end_of_packet, skips the checksum
	
isr_packet_receiver_500:	;calculates the CRC of the packet
	mov temp3, temp1	
	eor temp1, packetchecksum		; IN PYTHON: crc = input^crc
	
	ldi ZH, high(crc_table*2)		;loads CRC table location into Z pointer
	ldi ZL, low(crc_table*2)
	add ZL, temp1					;offsets Z pointer by value of temp1
	adc ZH, numzero
	lpm packetchecksum, Z			;stores new checksum in packetchecksum


	inc packetposition
	rjmp isr_packet_receiver_exit

	
isr_packet_receiver_600:	;check if packet is for us
	
	mov temp3, temp1				;stores a copy of the received byte in temp3
	
	ldi ZH, high(rx_packet_location+packet_startbyte)		;loads packet start byte into temp1
	ldi ZL, low(rx_packet_location+packet_startbyte)
	ld temp1, Z
	
	cpi temp1, packet_broadcast_startbyte
	breq isr_packet_receiver_700							;broadcast packet, skips address check.
	
	cpi temp1, packet_unicast_startbyte
	brne isr_packet_receiver_900							;packet bad!
	

	ldi ZH, high(rx_packet_location+packet_destaddress0)	;load destination address byte 0 into temp1
	ldi ZL, low(rx_packet_location+packet_destaddress0)
	ld temp1, Z
	
	ldi ZH, high(node_address_location)						;load node address byte 0 into temp2
	ldi ZL, low(node_address_location)	
	ld temp2, Z
	
	cp temp1, temp2
	brne isr_packet_receiver_900							;packet not destined for us!


	ldi ZH, high(rx_packet_location+packet_destaddress1)	;load destination address byte 0 into temp1
	ldi ZL, low(rx_packet_location+packet_destaddress1)
	ld temp1, Z
	
	ldi ZH, high(node_address_location+1)						;load node address byte 0 into temp2
	ldi ZL, low(node_address_location+1)	
	ld temp2, Z
	
	cp temp1, temp2
	brne isr_packet_receiver_900							;packet not destined for us!


	
	
isr_packet_receiver_700:			;check packet checksum
	cp temp3, packetchecksum			;see if checksum matches last byte
	brne isr_packet_receiver_900							;checksum bad. For now, ditch packet. Later, this could dispatch a resend-request.
	
	
isr_packet_receiver_800:			;packet is good, and destined for US!

	ldi     temp1, (0 << OCIE0B)|(0 << OCIE0A)|(0 << TOIE0)		;disables timer0 interrupts
    sts     TIMSK2, TEMP1

	sbi comm_status, packet_received	;sets packet waiting flag

	clr packetposition
	clr packetchecksum
	
	cbi comm_status, packet_inbound		;clears packet inbound flag
	cbi comm_status, packet_outbound

	rjmp isr_packet_receiver_exit
	
	
	
isr_packet_receiver_900:			;packet either not for us, or checksum is bad. Packet is tossed.

;	sts UDR0, numzero				;REMOVE

	clr packetposition
	clr packetchecksum
	
	cbi comm_status, packet_inbound		;sets packet inbound flag

	ldi     temp1, (0 << OCIE0B)|(0 << OCIE0A)|(0 << TOIE0)		;disables timer2 interrupts
    sts     TIMSK2, TEMP1

	
	
isr_packet_receiver_exit:
	pop temp2
	out SREG, temp2
	pop temp2
	pop temp1
	pop ZH
	pop ZL
	sei
	reti



;-- PACKET TRANSMITTER --
;
; This interrupt routine transmits bytes of a packet whenever they become avaliable.
;


isr_packet_transmitter_100:
	cli
	push ZL
	push ZH
	push temp1
	push temp2
	in temp2, SREG
	push temp2


	ldi temp1, (1<<TXC0)	;clears any potential transmission complete flags before enabling the interrupt.
	sts UCSR0A, temp1

	ldi temp1, (1<<RXEN0)|(1<<TXCIE0)|(1<<TXEN0)	;switches interrupt mode to interrupt on transmission completion
	sts UCSR0B, temp1
	
	sbi comm_status, packet_outbound				;set outgoing packet flag

	sbi comm_port, tx_enable						;enable RS-485 driver
	
isr_packet_transmitter_200:	;checks if end of packet has been reached

	cp packetposition, packetlength
	brsh isr_packet_transmitter_500
	
	
isr_packet_trasmitter_300:	;transmits next byte in packet

	ldi ZH, high(tx_packet_location)
	ldi ZL, low(tx_packet_location)
	add ZL, packetposition
	adc ZH, numzero
	
	ld temp1, Z					;loads current byte of the packet into temp1
	sts UDR0, temp1				;begins transmission of byte
	inc packetposition
	

isr_packet_transmitter_400:		;calculate checksum

	eor temp1, packetchecksum		; IN PYTHON: crc = input^crc
	
	ldi ZH, high(crc_table*2)		;loads CRC table location into Z pointer
	ldi ZL, low(crc_table*2)
	add ZL, temp1					;offsets Z pointer by value of temp1
	adc ZH, numzero
	lpm packetchecksum, Z			;stores new checksum in packetchecksum
	
	rjmp isr_packet_transmitter_exit


isr_packet_transmitter_500:
	cp packetposition, packetlength
	brne isr_packet_transmitter_600		;packet position is greater than packet length, closing time.
	
	sts UDR0, packetchecksum			;transmit packet checksum
	inc packetposition
	rjmp isr_packet_transmitter_exit
	
isr_packet_transmitter_600:
	
	ldi temp1, (1<<RXEN0)|(1<<RXCIE0)|(1<<TXEN0)	;disable transmitter interrupts
	sts UCSR0B, temp1
	
	cbi comm_Status, packet_outbound	;clear outgoing packet flag
	cbi comm_port, tx_enable			;disable RS-485 driver
	clr packetposition
	clr packetchecksum
	
isr_packet_transmitter_exit:
	pop temp2
	out SREG, temp2
	pop temp2
	pop temp1
	pop ZH
	POP ZL
	sei
	reti





;--RECEIVER WATCHDOG--

isr_packet_timeout_100:

	cli
	push temp1
	push temp2
	in temp2, SREG
	push temp2
	
	
;isr_packet_timeout_200:
	
	mov temp1, rcv_Watchdog
	cpi temp1, watchdog_trip
	brne isr_packet_timeout_exit


isr_packet_timeout_300:
	clr rcv_watchdog
	clr packetposition			;clears packet control registers
	clr packetchecksum
	ldi     temp1, (0 << OCIE0B)|(0 << OCIE0A)|(0 << TOIE0)		;disables timer0 interrupts
    sts     TIMSK2, TEMP1
	
	sbi led_toggle, led		;REMOVE AFTER DEBUG

isr_packet_timeout_exit:
	inc rcv_watchdog
	pop temp2
	out SREG, temp2
	pop temp2
	pop temp1
	sei
	reti

;--SERVO CONTROL INTERRUPT--
;
; this interrupt drives the servo control pins.


isr_servo_100:

	cli
	push temp1
	push temp2
	in temp2, SREG
	push temp2

	

	sbic comm_status, intermission	
	rjmp isr_servo_300				;if coming out of an intermission, skips to isr_servo_100
	

isr_servo_200:		;ending a pulse, begin an intermission

	cbi servo1_port, servo1		;clears servo 1 control pin
	sbi comm_status, intermission	;indicate beginning of intermission
	
	;intermission word = 5000
	
	ldi temp1, 138	;intermission low byte
	ldi temp2, 19	;intermission high byte
	
	sts OCR1AH, temp2
	sts OCR1AL, temp1
	
	rjmp isr_servo_exit
	
isr_servo_300:		;ending an intermission, beginning a pulse.
	sbi servo1_port, servo1
	cbi comm_status, intermission	;indicates beginning of pulse
	
	sts OCR1AH, servo1_byte1	;servo1 high byte
	sts OCR1AL, servo1_byte0	;servo1 low byte
	
	

isr_servo_exit:
	pop temp2
	out SREG, temp2
	pop temp2
	pop temp1
	sei
	reti
	
	

;--MAIN PROGRAM--------------------------------------------------------------------

reset:
    clr numzero

;CONFIGURE STACK

    ldi temp1, high(RAMEND)
    out SPH, temp1
    ldi temp1, low(RAMEND)
    out SPL, temp1

;CONFIGURE CLOCK

    ldi     temp1, (1 << CLKPCE)
    sts     clkpr, temp1			;enables writing to the clock prescaler
    sts     clkpr, numzero 			;sets the clock prescaler to 1:1


;INITIALIZE REGISTERS

	clr packetposition
	clr packetlength
	clr packetchecksum
	clr rcv_watchdog
	
;INITIALIZE PORTS

;  set pull-ups. These get set first, when every pin is an input.

    ldi temp1, portb_pullups
    out PORTB, temp1
	ldi temp1, portc_pullups
	out PORTC, temp1
	ldi temp1, portd_pullups
	out PORTD, temp1


; set directions.

	ldi temp1, portb_directions
	out DDRB, temp1
	ldi temp1, portc_directions
	out DDRC, temp1
	ldi temp1, portd_directions
	out DDRD, temp1
	

; set initial values
	
	out comm_status, numzero
	
	
	

; initialize registers


;CONFIGURE USART0

	ldi		temp1, 0
	ldi		temp2, 12			;19.6K @ 20MHz = 64, 115.2K @ 20MHz = 10, 115.2K @ 16MHz = 8, 76.8k @ 16MHz = 12
	
	sts 	UBRR0H, temp1	;19.6k baud at a clock of 20mhz
	sts 	UBRR0L, temp2
	
	ldi		temp1, (1<<RXCIE0)|(1<<RXEN0)|(1<<TXEN0)	;enable receiver and transmitter, receiver interrupts.
	sts		UCSR0B, temp1
	
	ldi		temp1, (1<<UCSZ01)|(1<<UCSZ00)		;asynchronous uSAURT
	sts		UCSR0C, temp1
	
;CONFIGURE TIMER2 AS WATCHDOG FOR RECEIVER
;19200 BAUD = 1041 clock ticks / bit. With a frame size of 10 bits, this is ~10000 ticks per byte.
    
    ldi     temp1, (0 << COM0A1)|(0 << COM0A0)|(0 << COM0B1)|(0 << COM0B0)|(0 << WGM01)|(0 << WGM00)	;configures timer0 in normal mode
    sts     TCCR2A, TEMP1

    ldi     temp1, (0 << FOC0A)|(0 << FOC0B)|(0 << WGM02)|(1 << CS02)|(0 << CS01)|(1 << CS00)	;configures prescaler as clk/64, ~16k ticks between interrupts
    sts     TCCR2B, TEMP1
 
    ldi     temp1, (0 << OCIE0B)|(0 << OCIE0A)|(0 << TOIE0)		;disables timer0 interrupts (for now)
    sts     TIMSK2, TEMP1
	

;CONFIGURE TIMER1 FOR RANDOM IP SELECTION

    ldi     temp1, (0 << COM1A1)|(0 << COM1A0)|(0 << COM1B1)|(0 << COM1B0)|(0 << WGM11)|(0 << WGM10)	;configures timer1 in normal mode
    sts     TCCR1A, TEMP1

    ldi     temp1, (0 << ICNC1)|(0 << ICES1)|(0 << WGM12)|(0 << CS12)|(0 << CS11)|(1 << CS10)	;configures prescaler as clk/1
    sts     TCCR1B, TEMP1




	
;LOAD HARD-CODED NODE ADDRESS INTO MEMORY

	ldi XH, high(node_address_location)
	ldi XL, low(node_address_location)
	ldi temp1, node_address0
	st X+, temp1
	ldi temp1, node_address1
	st X+, temp1
	
	
;LOAD DEFAULT SERVO POSITIONS INTO MEMORY

	ldi servo1_byte0, 119		;lplaces servo 1 in mid-stroke, with an initial timer value of 375
	ldi servo1_byte1, 1
	

;ENABLE GLOBAL INTERRUPTS
    sei					
	

;GENERATE IP ADDRESS


generate_ip_100:
	sbis button_input, button
	rjmp generate_ip_200
	inc temp1
	cpi temp1, 255
	brne generate_ip_100
	inc temp2
	cpi temp2, 255
	brne generate_ip_100
	inc temp3
	cpi temp3, 20			;shoud be roughly 0.5 second delay at 20MHz
	brne generate_ip_100
	
	sbi led_toggle, led		;toggle led
	clr temp3
	rjmp generate_ip_100	
	
	
generate_ip_200:

	cli
	lds temp1, TCNT1L
	lds temp2, TCNT1H
	sei


	ldi XH, high(node_address_location)
	ldi XL, low(node_address_location)
	st X+, temp1
	st X, temp2
	
	cbi led_output, led


;configure timer1 for servo control.

    ldi     temp1, (0 << COM1A1)|(0 << COM1A0)|(0 << COM1B1)|(0 << COM1B0)|(0 << WGM11)|(0 << WGM10)	;configures timer1 in CTC mode, on oCR1A
    sts     TCCR1A, TEMP1


    ldi     temp1, (0 << ICNC1)|(0 << ICES1)|(0 << WGM13)|(1 << WGM12)|(0 << CS12)|(1 << CS11)|(1 << CS10)	;configures prescaler as clk/64
    sts     TCCR1B, TEMP1
	
	sts OCR1AH, servo1_byte1
	sts OCR1AL, servo1_byte0	;loads initial timer value into OCR1A
	
	ldi		temp1, (1 << OCIE1A)	;enables counter interrupts
	sts		TIMSK1, temp1



;MAIN PROGRAM

main_program:

	rcall packet_router
	
	rjmp main_program
	

	
	
	


;--ROUTINES--------------------------------------------------------------------


;-- PACKET ROUTER --
;
; Checks for the presence of a new packet, then calls the appropriate hander based on the port.

packet_router:
	sbis comm_status, packet_received			;checks if packet is waiting and hasn't yet been serviced.
	ret

packet_router_100:
	cbi comm_status, packet_received		;clears packet waiting flag
	
	ldi ZH, high(rx_packet_location+packet_destport)	;load destination port into temp2
	ldi ZL, low(rx_packet_location+packet_destport)
	ld temp2, Z
		
	ldi ZH, high(port_table*2)		;loads port table location into Z pointer
	ldi ZL, low(port_table*2)	
	
packet_router_200:		;searches for entry in table which matches packet destination port number.					
	
	lpm temp1, Z			;loads port number of table entry into temp1

	cpi temp1, 0
	breq packet_router_400		;checks for end of port table
	
	cp temp1, temp2				;tests table entry port number for match
	breq packet_router_300
	
	adiw ZL, 4				;forwards table location to the port address of the next row
	
	rjmp packet_router_200
	

packet_router_300:
	adiw ZL, 2			;forwards table pointer to the service routine location.
	lpm temp1, Z+			;loads byte0 of service routine location into temp1
	lpm temp2, Z			;loads byte1 of service routine location into temp2

	mov ZL, temp1
	mov ZH, temp2
	
	ijmp					;jumps to service routine. Note that icall is not used, because it will be used by the routine to return to the main loop.

packet_router_400:
	clr temp1
;	sts UDR0, temp1		;REMOVE
	ret



;-- GET RX BYTE --
;
;inputs: none
;outputs: received byte in temp1
;
;destroys: temp1


get_rx_byte:

	lds temp1, UCSR0A	;wait for receiver byte
	sbrs temp1, RXC0
	rjmp get_rx_byte
	lds temp1, UDR0
	ret
	

;-- PUT TX BYTE --
;
;inputs: temp1
;outputs: none
;
;destroys: temp1 and temp2

put_tx_byte:

	lds temp2, UCSR0A	;wait for xmitter ready
	sbrs temp2, UDRE0
	rjmp put_tx_byte
	sts UDR0, temp1		;xmitting a move complete byte
	ret
	


;-- TRANSMIT PACKET --
;
; initiates transmission of packet stored in the transmit buffer

transmit_packet:
	
	clr packetchecksum		;resets receiver/transmitter registers in preparation for xmit.
	clr packetposition	

	ldi ZH, high(tx_packet_location+packet_length)	;loads packet length into length register
	ldi ZL, low(tx_packet_location+packet_length)
	ld packetlength, Z
	
	
	ldi temp1, (1<<RXEN0)|(1<<TXEN0)|(1<<UDRIE0)	;enables transmitter, disables receiver. Also enables buffer empty interrupt to trigger first call.
	sts UCSR0B, temp1
		
	ret

;-- BUILD VM URL REQUEST RESPONSE--
;
;sends a multicast return packet with the module's vm url in the paoad

build_vmurl_response_100:

	ldi XH, high(node_address_location)			;gets our address.
	ldi XL, low(node_address_location)
	ld temp1, X+
	ld temp2, X+
	

	ldi XH, high(tx_packet_location)			;initializes base tx packet
	ldi XL, low(tx_packet_location)

	ldi temp3, 138		;multicast, to escape other nodes from this routine
	st X+, temp3
	
	st X+, temp1
	st X+, temp2
	
	ldi temp1, 10		;vmurl_request port
	st X+, temp1
	
	
	ldi ZH, high(rx_packet_location+packet_srcaddress0)		;loads source address of triggering packet.
	ldi ZL, low(rx_packet_location+packet_srcaddress0)
	ld temp1, Z+
	ld temp2, Z
	
	
	st X+, temp1
	st X+, temp2
	ldi temp1, 0		;null port, so that if isn't caught, it doesn't do anything. could change this later.
	st X+, temp1
	ldi temp1, 44		;packet length, 8 (minimum length) + 36 (length of vmurl)
	st X+, temp1
	

	ldi ZH, high(vmurl_data*2)		;loads vmurl location into Z pointer
	ldi ZL, low(vmurl_data*2)
	
	clr temp2		;clears temp2 to be used as a counter for end-of-message			
	
build_vmurl_response_200:	;fills the payload with the vm_url
	lpm temp1, Z+
	st X+, temp1
	inc temp2
	cpi temp2, 36
	brne build_vmurl_response_200
	
	ret


;--SENDS A MOVE REQUEST RESPONSE PACKET-----------
;

build_servo_return_packet:

	mov temp4, temp3					;turns out temp3 is used, so copied to temp4 (not valid anyways for ldi)
	
	ldi XH, high(node_address_location)			;gets our address.
	ldi XL, low(node_address_location)
	ld temp1, X+
	ld temp2, X+
	

	ldi XH, high(tx_packet_location)			;initializes base tx packet
	ldi XL, low(tx_packet_location)

	ldi temp3, 74		;unicast
	st X+, temp3
	
	st X+, temp1
	st X+, temp2
	
	ldi temp1, 7		;source port is servo command
	st X+, temp1
	
	
	ldi ZH, high(rx_packet_location+packet_srcaddress0)		;loads source address of triggering packet.
	ldi ZL, low(rx_packet_location+packet_srcaddress0)
	ld temp1, Z+
	ld temp2, Z
	
	
	st X+, temp1
	st X+, temp2
	ldi temp1, 6		;send packet to move response port
	st X+, temp1
	ldi temp1, 19		;packet length, 8 (minimum length) + 1 (length of response)
	st X+, temp1
	
	ldi temp4, 72
	
	st X+, temp4		;confirmation byte: 72!
	ret
	
	
;--SERVICE ROUTINES---------------------------------------------------------------------


;--RETURN VM URL--
;
;This service routine sends an outgoing packet 

svc_vmurl:
	rcall build_vmurl_response_100				;build response first, because rx buffer gets wiped by any further incoming packets.

	clr temp1
	clr temp2
	clr temp3

svc_vmurl_200:
	sbic comm_status, packet_received			;checks if packet is waiting, assumes that this is the response from the selected node.
	rjmp svc_vmurl_400
	
	sbis button_input, button
	rjmp svc_vmurl_300
	inc temp1
	cpi temp1, 255
	brne svc_vmurl_200
	inc temp2
	cpi temp2, 255
	brne svc_vmurl_200
	inc temp3
	cpi temp3, 10			;shoud be roughly 0.5 second delay at 20MHz
	brne svc_vmurl_200
	
	sbi led_toggle, led		;toggle led
	clr temp3
	rjmp svc_vmurl_200
	
	
	
svc_vmurl_300:
	cbi led_output, led		;turn led off
	rcall transmit_packet
	ret
	
svc_vmurl_400:
	cbi comm_status, packet_received			;clears packet-received notification, assuming either one was received or this node was selected.			
	ret
	

;--PROCESS MOTOR MOVE COMMAND--
;
;This service routine copies a motion command to the buffer, and then begins execution of the command if one isn't already running.


svc_servo_control:

	ldi ZH, high(rx_packet_location+packet_payload)		;loads payload location into Z
	ldi ZL, low(rx_packet_location+packet_payload)
	
	cli	;disable global interrupts, since writing to servo control bytes.
	
	ld servo1_byte0, Z+		;loads servo 1 control byte 0
	ld servo1_byte1, Z		;loads servo 1 control byte 1
	
	sei	;enables global interrupts
	

	rcall build_servo_return_packet
	rcall transmit_packet
	
	ret
	



;--PORT TABLE---------------------------------------------------------------------------
;					note: stored little-endian, so least significant byte of each word is stored first.

port_table:
;		PORT VALUE			SERVICE ROUTINE				PORT DESCRIPTION
.dw		7,					svc_servo_control			;servo control packet
.dw 	10,					svc_vmurl					;sends a return packet with a URL pointing to the VM of the node.
.dw 0													;when 0 is encountered, the lookup routine knows table end has been reached.
	
	
;--VM URL-----------------------------------------------------------------------------

vmurl_data:
.db "http://mtm.cba.mit.edu/vm/076-007.py"		;length is 36 characters

;--DATA TABLES------------------------------------------------------------------------

crc_table:
.db 0, 7
.db 14, 9
.db 28, 27
.db 18, 21
.db 56, 63
.db 54, 49
.db 36, 35
.db 42, 45
.db 112, 119
.db 126, 121
.db 108, 107
.db 98, 101
.db 72, 79
.db 70, 65
.db 84, 83
.db 90, 93
.db 224, 231
.db 238, 233
.db 252, 251
.db 242, 245
.db 216, 223
.db 214, 209
.db 196, 195
.db 202, 205
.db 144, 151
.db 158, 153
.db 140, 139
.db 130, 133
.db 168, 175
.db 166, 161
.db 180, 179
.db 186, 189
.db 199, 192
.db 201, 206
.db 219, 220
.db 213, 210
.db 255, 248
.db 241, 246
.db 227, 228
.db 237, 234
.db 183, 176
.db 185, 190
.db 171, 172
.db 165, 162
.db 143, 136
.db 129, 134
.db 147, 148
.db 157, 154
.db 39, 32
.db 41, 46
.db 59, 60
.db 53, 50
.db 31, 24
.db 17, 22
.db 3, 4
.db 13, 10
.db 87, 80
.db 89, 94
.db 75, 76
.db 69, 66
.db 111, 104
.db 97, 102
.db 115, 116
.db 125, 122
.db 137, 142
.db 135, 128
.db 149, 146
.db 155, 156
.db 177, 182
.db 191, 184
.db 173, 170
.db 163, 164
.db 249, 254
.db 247, 240
.db 229, 226
.db 235, 236
.db 193, 198
.db 207, 200
.db 221, 218
.db 211, 212
.db 105, 110
.db 103, 96
.db 117, 114
.db 123, 124
.db 81, 86
.db 95, 88
.db 77, 74
.db 67, 68
.db 25, 30
.db 23, 16
.db 5, 2
.db 11, 12
.db 33, 38
.db 47, 40
.db 61, 58
.db 51, 52
.db 78, 73
.db 64, 71
.db 82, 85
.db 92, 91
.db 118, 113
.db 120, 127
.db 106, 109
.db 100, 99
.db 62, 57
.db 48, 55
.db 34, 37
.db 44, 43
.db 6, 1
.db 8, 15
.db 26, 29
.db 20, 19
.db 174, 169
.db 160, 167
.db 178, 181
.db 188, 187
.db 150, 145
.db 152, 159
.db 138, 141
.db 132, 131
.db 222, 217
.db 208, 215
.db 194, 197
.db 204, 203
.db 230, 225
.db 232, 239
.db 250, 253
.db 244, 243