robot.asm

;***********************************************************
;*
;*  robot.asm
;*
;*  This is the RECEIVE file for Lab 8 of ECE 375
;*
;***********************************************************
;*
;*   Author: Enter your name
;*     Date: Enter Date
;*
;***********************************************************

.include "m128def.inc"      ; Include definition file

;***********************************************************
;*  Internal Register Definitions and Constants
;***********************************************************
.def  mpr = r16       ; Multi-Purpose Register
.def  receiveState = r17 ;register to hold state for receive
.def  mpr2 = r18

;registers used by blocking wait routine
.def waitcnt = r19
.def ilcnt = r20
.def olcnt = r21

.equ wait_time = 100

.equ STATE_ADDR_RECEIVED = 1
.equ STATE_NO_ADDR = 2

.equ  WskrR = 0       ; Right Whisker Input Bit
.equ  WskrL = 1       ; Left Whisker Input Bit
.equ  EngEnR = 4        ; Right Engine Enable Bit
.equ  EngEnL = 7        ; Left Engine Enable Bit
.equ  EngDirR = 5       ; Right Engine Direction Bit
.equ  EngDirL = 6       ; Left Engine Direction Bit

.equ MoveForwardFlags = (1<<EngEnL | 1<<EngEnR | 1<<EngDirR | 1<<EngDirL)
.equ TurnRightFlags = (1<<EngEnL | 1<<EngEnR | 0<<EngDirR | 1<<EngDirL)
.equ TurnLeftFlags = (1<<EngEnL | 1<<EngEnR | 1<<EngDirR | 0<<EngDirL)
.equ HaltFlags = (0<<EngEnL | 0<<EngEnR)
.equ MoveBackwardsFlags = (1<<EngEnL | 1<<EngEnR | 0<<EngDirR | 0<<EngDirL)


.equ  BotAddress = 0b00100100

;command codes
.equ  MoveForwardCode = 0b10110000
.equ  MoveBackCode =  0b10000000
.equ  TurnRightCode = 0b10100000
.equ  TurnLeftCode = 0b10010000 
.equ  HaltCode =  0b11001000

;***********************************************************
;*  Start of Code Segment
;***********************************************************
.cseg             ; Beginning of code segment

;***********************************************************
;*  Interrupt Vectors
;***********************************************************
.org  $0000         ; Beginning of IVs
    rjmp  INIT      ; Reset interrupt

;Should have Interrupt vectors for:
;- Left whisker
.org $0002
  rcall HitRightInterrupt
  reti

;- Right whisker
.org $0005
  rcall HitLeftInterrupt
  reti

;- USART receive
.org $003C
  rcall CommandReceiveInterrupt
  reti

.org  $0046         ; End of Interrupt Vectors


;***********************************************************
;*  Program Initialization
;***********************************************************
INIT:
  ;Stack Pointer
  outi SPH, high(RAMEND)
  outi SPL, low(RAMEND)

  ;I/O Ports
    ;set PORTB (output) - engine control

    ;set PORTD (input) (?)

  ;USART1 ---------------------------------------------------
  ;Set baudrate at 2400bps
  ldi mpr, high(832)
  sts UBRR1H, mpr
  ldi mpr, low(832)
  out UBRR1L, mpr

  ;Enable receiver and enable receive interrupts
  ldi mpr, (1<<RXEN1 | 1<<RXCIE1)
  
  ;Set frame format: 8 data bits, 2 stop bits
  ldi mpr, (0<<UMSEL0 | 1<<USBS0 | 1 <<UCSZ01)
  sts UCSR1C, mpr

  ;External Interrupts -------------------------------------
  ;Set the External Interrupt Mask
  
  ;Set the Interrupt Sense Control to falling edge detection

  ;Other

;***********************************************************
;*  Main Program
;***********************************************************
MAIN:
  ;TODO: ???
    rjmp  MAIN

;***********************************************************
;*  Functions and Subroutines
;***********************************************************

HitLeftInterrupt:

  ret

HitRightInterrupt:
  ret

CommandReceiveInterrupt:
  push mpr
  ;get the USART data
  in mpr, UDR1

  ;if first bit is 0:
  ;TODO
  sbrs mpr, 0
  rjmp FirstBitOne

    ;if receiveState == STATE_NO_ADDR:
    cpi receiveState, STATE_NO_ADDR
    brne FirstBitOne

      ;if mpr== BotAddress:
      cpi mpr, BotAddress
      brneq EndCommandReceivedInterrupt

        ;set receiveState = STATE_ADDR_RECEIVED
        ldi receiveState, STATE_ADDR_RECEIVED

        rjmp EndCommandReceivedInterrupt

  FirstBitOne ;TODO rename this
  ;if first bit is 1:
  ;TODO
  
    ;if receiveState == STATE_ADDR_RECEIVED:
    cpi receiveState, STATE_ADDR_RECEIVED
    brne EndCommandReceivedInterrupt

      ;execute command in mpr
      cpi mpr, TurnLeftCode
      breq TurnLeft
      cpi mpr, TurnRightCode
      breq TurnRight
      cpi mpr, MoveForwardCode
      breq MoveForward
      cpi mpr, MoveBackCode
      breq MoveBack
      cpi mpr, HaltCode
      breq Halt

      ;set receiveState = STATE_NO_ADDR
      ldi receiveState, STATE_NO_ADDR
  
  EndCommandReceivedInterrupt:
  pop mpr
  ret

MoveForward:
  push mpr
  ldi mpr, MoveForwardFlags
  out PORTB, mpr
  pop pr
  ret

TurnRight: 
  push mpr
  in mpr, PORTB
  ldi mpr, TurnRightFlags
  out PORTB, mpr
  pop mpr
  ret

Turn Left:
  push mpr
  ldi mpr, TurnLeftFlags
  out PORTB, mpr
  pop mpr
  ret

Halt:
  push mpr
  ldi mpr, HaltFlags
  out PORTB, mpr
  pop mpr
  ret

MoveBackward: 
  push mpr
  ldi mpr, MoveBackwardsFlags
  out PORTB, mpr
  pop mpr
  ret

HitRight:
  ;save program state
  push mpr
  push waitcnt
  in mpr, SREG
  push mpr

  ;move backwards 1 sec
  ldi mpr, MoveBackwardsFlags
  out PORTB, mpr
  ldi waitcnt, WTime
  rcall Wait

  ;turn left 1 sec
  ldi mpr, TurnLeftFlags
  out PORTB, mpr
  ldi waitcnt, wait_time
  rcall Wait

  ;continue moving forward
  ldi mpr, MoveForwardFlags
  out PORTB, mpr

  ;restore program state
  pop mpr
  out SREG, mpr
  pop waitcnt
  pop mpr

  ret

HitLeft:
  ;save program state
  push mpr
  push waitcnt
  in mpr, SREG
  push mpr

  ;move backwards 1 sec
  ldi mpr, MoveBackwardsFlags
  out PORTB, mpr
  ldi waitcnt, WTime
  rcall Wait

  ;turn right 1 sec
  ldi mpr, TurnRightFlags
  out PORTB, mpr
  ldi waitcnt, wait_time
  rcall Wait

  ;continue moving forward
  ldi mpr, MoveForwardFlags
  out PORTB, mpr

  ;restore program state
  pop mpr
  out SREG, mpr
  pop waitcnt
  pop mpr

  ret

Freeze:
  ;TODO


;******************
; wait subroutine
;******************
Wait:
  push waitcnt
  push ilcnt
  push olcnt

Loop:
  ldi olcnt, 224
OLoop: 
  ldi ilcnt, 237
ILoop: 
  dec ilcnt
  brne ILoop
  dec  olcnt
  brne OLoop
  dec waitcnt
  brne Loop
  
  pop olcnt
  pop ilcnt
  pop waitcnt
  ret
;***********************************************************
;*  Stored Program Data
;***********************************************************

;***********************************************************
;*  Additional Program Includes
;***********************************************************