; MazeRunner II Version 1.6 4/11/03 .nolist .include "c:\avrtools\AppNotes\m128def.inc" .list ;------ Display Commands ------------------------------------ .equ cmdClearDisplay = 0x80 ;To distingish these commands from .equ cmdGotoLine1 = 0x81 ;standard ASCII, the MSB is set. .equ cmdGotoLine2 = 0x82 .equ cmdGotoLine3 = 0x83 .equ cmdGotoLine4 = 0x84 .equ cmdCursorLeft = 0x85 .equ cmdCursorRight = 0x86 .equ cmdDisplayOff = 0x87 .equ cmdDisplayOn = 0x88 .equ cmdCursorOn = 0x89 .equ cmdCursorOff = 0x8A .equ cmdBlinkOn = 0x8B .equ cmdBlinkOff = 0x8C .equ cmdWakeup = 0x8D ;------ PortB Defines ---------------------------------------- .equ RunButton = 1 ;run button .equ SelectButton = 2 ;select button .equ ModeButton = 3 ;mode button ;------ PortD Defines ---------------------------------------- .equ Lencoder = 0 ;left encoder .equ Rencoder = 1 ;right encoder .equ GreenLED = 5 ;green LED .equ RedLED = 6 ;red LED .equ Fled = 7 ;Floor LED enable (active high) ;------ PortE Defines ---------------------------------------- .equ LowerRangeOut = 2 ;lower range sensor output .equ LowerRangeIn = 3 ;lower range sensor input .equ UpperRangeOut = 4 ;upper range sensor output .equ UpperRangeIn = 5 ;upper range sensor input .equ LeftBumper = 6 ;Left bumper switch .equ RightBumper = 7 ;Right bumper switch ;------ PortF Defines ---------------------------------------- .equ FrontLeftSensor = 0 ;range sensors .equ FrontRightSensor = 1 .equ RearLeftSensor = 2 .equ RearRightSensor = 3 .equ FrontFloorSensor = 4 .equ RearFloorSensor = 5 .equ BatteryMonitor = 6 ;------ Macros for Port control ------------------------------ .macro EnableUsensor cbi porte,UpperRangeIn .endm .macro DisableUsensor sbi porte,UpperRangeIn .endm .macro EnableLsensor cbi porte,LowerRangeIn .endm .macro DisableLsensor sbi porte,LowerRangeIn .endm .macro GreenLEDon cbi portd,GreenLED ;turn off green LED .endm .macro GreenLEDoff sbi portd,GreenLED ;turn off green LED .endm .macro RedLEDon cbi portd,RedLED ;turn off red LED .endm .macro RedLEDoff sbi portd,RedLED ;turn off red LED .endm .macro FloorLEDon sbi portd,Fled ;turn on floor LED's .endm .macro FloorLEDoff cbi portd,Fled ;turn off floor LED's .endm .macro LeftSensorsOn cbi porte,LsensorPower ;turn on left sensors .endm .macro LeftSensorsOff sbi porte,LsensorPower ;turn on left sensors .endm .macro RightSensorsOn cbi porte,RsensorPower ;turn on right sensors .endm .macro RightSensorsOff sbi porte,RsensorPower ;turn on right sensors .endm ;------ Mode Constants -------------------------------------- .equ mRun = 0 ;run the maze .equ mSingleStep = 1 ;debug mode .equ mDiagnostic = 2 ;display diagnostic information .equ mSensorTest = 3 ;display all sensor values .equ mAlign = 4 ;wall alignment test .equ mSpinTest = 5 ;do 4 90 degree spins .equ mSpeedTest = 6 ;test the PWM speed control .equ mSetup = 7 ;select the direction priorities .equ mDataDump = 8 ;send data out serial port .equ mLastMode = 9 ;last mode constant ;------ Flag Bits --------------------------------------- .equ Tick = 0 ;Timer Tick bit .equ SingleStep = 1 ;run the single step mode .equ Jogging = 2 ;a course adjustment is in progress .equ Bumper = 3 ;bumper flag .equ BallFound = 4 ;ball flag .equ FrontWhite = 5 ;front floor sensor flag .equ RearWhite = 6 ;rear floor sensor flag ;------ Scanner States ---------------------------- .equ ssStart = 0 ;the sensor hasn't started .equ ssEnabled = 1 ;sensor is enabled and measuring .equ ssWait = 2 ;Sensor finshed, now waiting for next cycle ;------ Heading Constants ------------------------- .equ North = 0 .equ East = 1 .equ South = 2 .equ West = 3 ;------ Setup Constants --------------------------- .equ sNESW = 0 .equ sESWN = 1 .equ sSWNE = 2 .equ sWNES = 3 .equ sLastSetup = 4 ;------ Register Defines -------------------------------- .def SaveSreg = r0 ;used in all ISR's .def ISRtemp = r1 ;can be used in ISR routines .def ISRAdc = r2 ;used in ADC ISR .def ScanValue = r3 ;IR scanner reading .def MyLoop = r4 ;used for various loops .def LeftSpeed = r5 ;current left motor speed .def RightSpeed = r6 ;current right motor speed .def SensorTicks = r7 ;Used in timer routines .def FrontReading = r8 ;used in Lineup routines .def NewHeading = r9 ;used in ChangeHeading .def MScount = r10 ;used in MoveOneCell .def Voltage = r11 ;used in DisplayBattery .def JogTimer = r12 ;used by speed test .def Setup = r13 ;contains setup (direction priority) .def Acc = r16 ;used as accumulator .def Mode = r17 ;running mode .def Lencoder = r18 ;Left encoder count .def Rencoder = r19 ;Right encoder count .def Flags = r20 ;Flag bits .def SensorState = r21 ;Used by DoSensor task .def ADChi = r22 ;used in ADC ISR .def Heading = r23 ;current heading .def Cell = r24 ;current cell information .def MoveCount = r25 ;used in MoveOneCell CBL ; r26 ;X register used as cell pointer ; r28 ;Y register used to store status block with ADC results ; r30 ;Z register used for IJMPs ;------ Ram Defines ------------------------------------- .equ StatusBlock = 0x100 ;begining of status block .equ sbFrontLeft = 0x100 ;current value for Front Left sensor .equ sbFrontRight = 0x101 ;current value for Front Right sensor .equ sbRearLeft = 0x102 ;current value for Rear Left sensor .equ sbRearRight = 0x103 ;current value for Rear Right sensor .equ sbFrontFloor = 0x104 ;current value for Front Floor sensor .equ sbRearFloor = 0x105 ;current value for Rear Floor sensor .equ sbBattery = 0x106 ;battery voltage .equ sbUpperSensor = 0x107 ;Forward Upper IR sensor .equ sbLowerSensor = 0x108 ;Forward Lower IR sensor .equ sbRunningMode = 0x109 ;Current Running Mode (Explore, Retreat, Finish) .equ sbRightFullSpeed = 0x10a ;full speed setting for right motor .equ sbLeftFullSpeed = 0x10b ;full speed setting for left motor .equ CellArray = 0x200 ;cell used to map the maze .equ DataLog = 0x600 ;logs sensor data ;------ Interrupt Vactor Table -------------------------- .cseg .org 0 jmp start ;RESET .org 2 jmp INT0isr ;INT0 External Interrupt Request .org 4 jmp INT1isr ;INT1 External Interrupt Request .org 6 reti ;INT2 External Interrupt Request .org 8 reti ;INT3 External Interrupt Request .org 0xa reti ;INT4 External Interrupt Request .org 0xc reti ;INT5 External Interrupt Request .org 0xe jmp INT6isr ;INT6 External Interrupt Request (bumper) .org 0x10 jmp INT6isr ;INT7 External Interrupt Request (bumper) .org 0x12 reti ;TIMER2 COMP Timer/Counter2 Compare Match .org 0x14 rjmp Timer2 ;TIMER2 OVF Timer/Counter2 Overflow .org 0x16 reti ;TIMER1 CAPT Timer/Counter1 Capture Event .org 0x18 reti ;TIMER1 COMPA Timer/Counter1 Compare Match A .org 0x1a reti ;TIMER1 COMPB Timer/Counter1 Compare Match B .org 0x1c reti ;TIMER1 OVF Timer/Counter1 Overflow .org 0x1e jmp Timer0 ;TIMER0 COMP Timer/Counter0 Compare Match .org 0x20 reti ;TIMER0 OVF Timer/Counter0 Overflow .org 0x22 reti ;SPI, STC SPI Serial Transfer Complete .org 0x24 reti ;USART0, RX USART0, Rx Complete .org 0x26 reti ;USART0, UDRE USART0 Data Register Empty .org 0x28 reti ;USART0, TX USART0, Tx Complete .org 0x2a jmp ADCisr ;ADC ADC Conversion Complete .org 0x2c reti ;EE READY EEPROM Ready .org 0x2e reti ;ANALOG COMP Analog Comparator .org 0x30 reti ;TIMER1 COMPC Timer/Countre1 Compare Match C .org 0x32 reti ;TIMER3 CAPT Timer/Counter3 Capture Event .org 0x34 reti ;TIMER3 COMPA Timer/Counter3 Compare Match A .org 0x36 reti ;TIMER3 COMPB Timer/Counter3 Compare Match B .org 0x38 reti ;TIMER3 COMPC Timer/Counter3 Compare Match C .org 0x3a reti ;TIMER3 OVF Timer/Counter3 Overflow .org 0x3c reti ;USART1, RX USART1, Rx Complete .org 0x3e reti ;USART1, UDRE USART1 Data Register Empty .org 0x40 reti ;USART1, TX USART1, Tx Complete .org 0x42 reti ;TWI Two-wire Serial Interface .org 0x44 reti ;SPM READY Store Program Memory Ready ;**************************************************************** ; Motor Speed Tables * ;**************************************************************** SpeedTableL: .db 0x00,0xa6,0xa4,0xa2,0xa0,0x9e,0x9c,0x99 ;left speed values 0-7 SpeedTableR: .db 0x00,0xa8,0xaa,0xac,0xae,0xb0,0xb2,0xb4 ;right speed values 0-7 ;**************************************************************** ; Timer0 - Timer Interrupt Service Routine * ; Entry occurs every 1ms * ;**************************************************************** Timer0: in SaveSreg,sreg ;save status register inc SensorTicks ;inc the tick counter sbr Flags,1< 255 steps) rcall ClearDisplay rcall Delay100ms ;wait for display to catch up rcall DisplayHeading ;show the current heading ldi r16,cmdGotoLine2 rcall DisplayCellData ;display current cell ldi r16,cmdGotoLine3 rcall DisplayCellData ;display current cell-1 rcall DisplayRom .db cmdGotoLine4,"L=",0 lds r16,sbLowerSensor rcall DisplayByte rcall DisplayRom .db " U=",0 lds r16,sbUpperSensor rcall DisplayByte rcall DisplayRom .db " L=",0 lds r16,sbRearLeft rcall DisplayByte rcall DisplayRom .db " R=",0 lds r16,sbRearRight rcall DisplayByte pop xl ;restore cell pointer ret DisplayCellData: push Heading ;save heading rcall DisplayChar mov r16,xl ;get the current cell value rcall DisplayByte ;display the cell number rcall DisplayRom .db " ",0 ldi r16,4 mov MyLoop,r16 ;load loop counter ld Heading,x ;get the cell mapping (use heading temporarly) DCD1: rol Heading ;move bit into carry rcall DisplayCarry ;display 1st bit rol Heading rcall DisplayCarry ;display 2nd bit ldi r16,' ' rcall DisplayChar dec MyLoop ;dec loop counter brne DCD1 ;keep looping dec xl ;point to previous location pop Heading ;restore data ret DisplayCarry: brcs Show1 ldi r16,'0' rcall DisplayChar ret Show1: ldi r16,'1' rcall DisplayChar ret DisplayHeading: mov r16,Heading ;get the current heading cpi r16,North breq dNorth cpi r16,East breq dEast cpi r16,South breq dSouth rcall DisplayRom .db "West ",0 rjmp DH1 dNorth: rcall DisplayRom .db "North",0 rjmp DH1 dEast: rcall DisplayRom .db "East ",0 rjmp DH1 dSouth: rcall DisplayRom .db "South",0 DH1: rcall DisplayRom .db " N E S W ",0,0 lds r16,sbRunningMode rcall DisplayChar ret ;**************************************************************** ; WeAreFinished - Here to indicate finish of maze * ;**************************************************************** WeAreFinished: rcall DisplayRom .db cmdClearDisplay,"Victory is Mine!",0 rcall StopMotors ldi r16,9 mov MyLoop,r16 Waf: GreenLEDon RedLEDoff rcall Delay100ms rcall Delay100ms GreenLEDoff RedLEDon rcall Delay100ms rcall Delay100ms dec MyLoop brne Waf GreenLEDoff RedLEDoff ;Ok we should be done, but just incase we'll wait 5 seconds ;after that, take off looking for the finish line FailSafe: ldi r16,20 mov MyLoop,r16 ;setup a 5 second wait FS1: ldi r16,250 rcall Delay dec MyLoop brne FS1 rjmp MR0 ;return to explore mode ;**************************************************************** ; ModeSensors - Display all the sensor values * ;**************************************************************** ModeSensors: rcall ClearDisplay ldi SensorState,ssStart FloorLEDon MS1: rcall Delay100ms rcall GetLowerSensor rcall GetUpperSensor rcall DisplayRom .db cmdGotoLine1,"Forward U=",0 lds r16,sbUpperSensor rcall DisplayByte rcall DisplayRom .db " L=",0,0 lds r16,sbLowerSensor rcall DisplayByte rcall DisplayRom .db cmdGotoLine2," Front L=",0 lds r16,sbFrontLeft rcall DisplayByte rcall DisplayRom .db " R=",0,0 lds r16,sbFrontRight rcall DisplayByte rcall DisplayRom .db cmdGotoLine3," Rear L=",0 lds r16,sbRearLeft rcall DisplayByte rcall DisplayRom .db " R=",0,0 lds r16,sbRearRight rcall DisplayByte rcall DisplayRom .db cmdGotoLine4," Floor F=",0 lds r16,sbFrontFloor rcall DisplayByte rcall DisplayRom .db " R=",0,0 lds r16,sbRearFloor rcall DisplayByte sbic pinb,RunButton ;test run button rjmp MS1 ;jump if button not pressed rcall WaitOnButtonRelease FloorLEDoff rjmp MainStart ;**************************************************************** ; ModeAlign - Align the robot to the walls using the wall * ; sensors. * ;**************************************************************** ModeAlign: rcall DisplayRom .db cmdClearDisplay,"Align to Wall ",cmdgotoLine2,"Press Left or Right",0 Ma1: sbis pinb,ModeButton ;test mode button rjmp Ma2 ;jump if button pressed sbis pinb,SelectButton ;test select button rjmp Ma3 ;jump if button pressed sbis pinb,RunButton ;test run button rjmp MaDone ;jump if button pressed rjmp Ma1 Ma2: rcall WaitOnButtonRelease rcall LineupRight rjmp Ma4 Ma3: rcall WaitOnButtonRelease rcall LineupLeft Ma4: rcall DisplayRom .db cmdgotoLine3," FL = ",0 lds r16,sbFrontLeft rcall DisplayByte rcall DisplayRom .db " FR = ",0 lds r16,sbFrontRight rcall DisplayByte rcall DisplayRom .db cmdgotoLine4," RL = ",0 lds r16,sbRearLeft rcall DisplayByte rcall DisplayRom .db " RR = ",0 lds r16,sbRearRight rcall DisplayByte rjmp Ma1 MaDone: rcall WaitOnButtonRelease rjmp MainStart ;**************************************************************** ; ModeSpinTest - Test spin accuracy * ;**************************************************************** ModeSpinTest: rcall DisplayRom .db cmdClearDisplay," Spin Test",cmdGotoLine4," Left Right ",0 MStst1: sbis pinb,ModeButton ;test mode button rjmp MSTright ;jump if button pressed sbis pinb,SelectButton ;test select button rjmp MSTleft ;jump if button pressed sbis pinb,RunButton ;test run button rjmp MSTrun ;jump if button pressed rjmp MStst1 MSTleft: ldi r16,4 mov MyLoop,r16 MSTL1: rcall LeftTurn rcall Delay100ms ;pause 100ms rcall Delay100ms ;pause 100ms dec MyLoop brne MSTL1 rjmp ModeSpinTest MSTright: ldi r16,4 mov MyLoop,r16 MSTR1: rcall RightTurn rcall Delay100ms ;pause 100ms rcall Delay100ms ;pause 100ms dec MyLoop brne MSTR1 rjmp ModeSpinTest MSTrun: rcall WaitOnButtonRelease rjmp MainStart ;**************************************************************** ; ModeDiagnostic - Display the battery voltage and * ; bumper status * ;**************************************************************** ModeDiagnostic: rcall ClearDisplay rcall DisplayRom .db cmdClearDisplay," Diagnostics ",0 rcall Delay100ms ;wait for the display to catch up cbr flags,1<