include "regs11.lib" const SERVO_OFF = $0C00 const MaxInters = 32 'Don't move below declare ch declare Strch declare Inter(MaxInters) declare InterNeeded declare InterLoc declare InterVar declare InterValue declare i declare j declare Str 'Don't move above declare l_amount declare r_amount declare Solved declare CurInter declare OldInter declare FullSpeed declare HalfSpeed declare HalfRSpeed declare HalfLSpeed declare AdjSpeed declare ChangeValue declare Sens declare right_button declare left_button declare left_speed declare right_speed declare wait_amount declare waitI declare state declare CurDir declare NewDir declare k interrupt $FFEC l_amount = l_amount + 1 pokeb tflg1, %00000011 end interrupt $FFEA r_amount = r_amount + 1 pokeb tflg1, %00000011 end main: pokeb baud, $30 pokeb sccr2, $0c pokeb tmsk1, %00000011 pokeb tctl2, %00001111 ' pokeb tctl2, %00001010 pokeb tflg1, %00000011 pokeb option, peekb(option) or $80 pokeb pactl, %01010000 pokeb pacnt, $00 interrupts on AdjSpeed = $02 left_button = 0 right_button = 0 state = 0 CurInter = $01 OldInter = $00 Solved = 0 gosub Init_Servos pokeb portb, %00000000 pokeb ddrc, %00000000 gosub Init_LCD do select state case 0 pokeb portb, peekb(portb) or %01000000 gosub Clear_LCD do gosub Move_Lower gosub read_analog FullSpeed = ChangeValue HalfSpeed = FullSpeed / 2 gosub NumberOut ChangeValue = peekb(portc) gosub NumberOut if left_button < $0080 gosub Full_Forward state = 2 exit endif loop endcase case 2 gosub Clear_LCD ChangeValue = peekb(portc) gosub NumberOut Sens = peekb(portc) if Sens and %01000001 = 0 gosub Intersection endif if Sens and %00000001 = 0 gosub Right_branch else if Sens and %01000000 = 0 gosub Left_branch endif endif Sens = peekb(portc) if Sens and %01111111 = %01111111 gosub Dead_End endif Sens = peekb(portc) if Sens and %00111110 = %00110110 gosub FULL_Forward else if Sens and %00000100 = 0 gosub Speed_Left else if Sens and %00010000 = 0 gosub Speed_Right else endif endif endif if peekb(portc) and %01111111 = 0 state = 3 endif endcase case 3 Solved = 1 CurInter = $01 OldInter = $01 gosub Stop_motor gosub Clear_LCD StrCh = '*' gosub CharOut pokeb portb, peekb(portb) or %01000000 do gosub read_analog loop while left_button > $0080 gosub Full_forward state = 2 endcase endselect loop end Dead_End: l_amount = 0 gosub turn_L do Sens = peekb(portc) if Sens and %01111111 <> %01111111 gosub Full_Forward return endif loop while l_amount <* 3 Gosub Turn_L gosub Turn_until_Line CurInter = OldInter gosub Full_Forward return Right_branch: l_amount = 0 gosub full_forward do Sens = peekb(portc) if Sens and %01000000 = 0 gosub Intersection return endif loop while l_amount <* 3 if peekb(portc) and %01111111 = %01111111 Gosub Turn_R gosub Turn_until_Line else gosub Clear_LCD Strch = 'D' gosub charout InterValue = %00000110 gosub ProcIntersec endif return Left_branch: l_amount = 0 gosub full_forward do Sens = peekb(portc) if Sens and %00000001 = 0 gosub Intersection return endif loop while l_amount <* 3 if peekb(portc) and %01111111 = %01111111 Gosub Turn_L gosub Turn_until_Line else gosub Clear_LCD Strch = 'F' gosub charout InterValue = %00000011 gosub ProcIntersec endif return Intersection: l_amount = 0 gosub full_forward do loop while l_amount <* 3 if peekb(portc) and %01111111 = 0 state = 3 return endif gosub Clear_LCD if peekb(portc) and %01111111 = %01111111 Strch = 'B' gosub charout InterValue = %00000101 gosub ProcIntersec else Strch = 'E' gosub charout InterValue = %00000111 gosub ProcIntersec endif return ProcIntersec: gosub Stop_Motor if solved = 0 if CurInter = OldInter Strch = '-' gosub charout InterLoc = OldInter Gosub GetInter ChangeValue = InterValue gosub NumberOut j = %00000001 for i = 1 to 3 if InterValue and j = j CurDir = j CurDir = RShft(CurDir) if CurDir = 0 CurDir = %00001000 endif CurDir = RShft(CurDir) if CurDir = 0 CurDir = %00001000 endif j = j xor %11111111 InterValue = InterValue and j if InterValue = 0 OldInter = OldInter - 1 CurInter = OldInter else CurInter = CurInter + 1 endif gosub PutInter exit endif j = LShft(j) next else Strch = '+' gosub charout CurDir = %00000010 OldInter = CurInter CurInter = CurInter + 1 InterLoc = OldInter gosub PutInter endif else InterLoc = OldInter Gosub GetInter CurDir = %00000010 CurInter = CurInter + 1 endif gosub Move_Lower ChangeValue = InterValue gosub NumberOut j = %00000001 NewDir = %00001000 for i = 1 to 3 if InterValue and j = j NewDir = j exit endif j = LShft(j) next if CurDir <> NewDir CurDir = RShft(CurDir) if CurDir = 0 CurDir = %00001000 endif if CurDir = NewDir Gosub Turn_L gosub Turn_Until_Line else Gosub Turn_R gosub Turn_Until_Line endif endif gosub WaitForRight gosub Full_Forward return NumberOut: Str = ChangeValue i = Str and %00001111 asm ldd var009 lsrd lsrd lsrd lsrd std var009 endasm gosub DigitOut Str = i gosub DigitOut return DigitOut: if Str < 10 Strch = Str + '0' else Strch = Str + 55 endif gosub CharOut return Speed_Right: HalfRSpeed = SERVO_OFF + HalfSpeed if right_speed <* HalfRSpeed right_speed = right_speed + AdjSpeed else left_speed = left_speed + AdjSpeed endif if left_speed >* SERVO_OFF left_speed = SERVO_OFF endif gosub setspeed return Speed_Left: HalfLSpeed = SERVO_OFF - HalfSpeed if left_speed >* HalfLSpeed left_speed = left_speed - AdjSpeed else right_speed = right_speed - AdjSpeed endif if right_speed <* SERVO_OFF right_speed = SERVO_OFF endif gosub setspeed return print_sensors: ' i = %01000000 ' gosub Move_Lower ' Str = peekb(portc) ' do ' if Str and i = i ' strch = '1' ' gosub charout ' else ' strch = '0' ' gosub charout ' endif ' i = rshft(i) ' loop while i <> %00000000 return read_analog: pokeb adctl, $10 WaitAmount = 15 gosub Wait left_button = peekb(adr1); ChangeValue = peekb(adr2); pokeb adctl, $14 WaitAmount = 15 gosub Wait right_button = peekb(adr1); return WaitForRight: do gosub read_analog loop while right_button > $0080 return Turn_until_Line: l_amount = 0 do loop while l_amount <* 3 do loop until peekb(portc) and %00010000 = 0 Gosub Stop_Motor return Turn_L: left_speed = SERVO_OFF - HalfSpeed right_speed = SERVO_OFF - HalfSpeed gosub setspeed return Turn_R: left_speed = SERVO_OFF + HalfSpeed right_speed = SERVO_OFF + HalfSpeed gosub setspeed return Stop_motor: left_speed = SERVO_OFF right_speed = SERVO_OFF gosub setspeed return Full_Forward: left_speed = SERVO_OFF - FullSpeed right_speed = SERVO_OFF + FullSpeed gosub setspeed return 'Half_Forward: ' left_speed = SERVO_OFF - HalfSpeed ' right_speed = SERVO_OFF + HalfSpeed ' gosub setspeed 'return setspeed: poke toc2, right_speed poke toc3, left_speed return Init_Servos: pokeb tctl1, $50 pokeb oc1m, $60 pokeb oc1d, $60 gosub Stop_motor return Init_LCD: wait_amount=60 for i=1 to 3 ch = $03 gosub out gosub wait next Str = addr(InitStr) for i = 1 to 11 ch = peekb(Str) gosub out Str = Str + 1 next return Clear_LCD: ch = $00 gosub out ch = $01 gosub out gosub charout return Move_Lower: ch = $0c gosub out ch = $00 gosub out return wait: for WaitI = 1 to* wait_amount next return CharOut: asm ldd #240 anda var001 andb var001+1 lsrd lsrd lsrd lsrd std var000 endasm ch = ch or $10 gosub out ch = Strch and %00001111 ch = ch or $10 gosub out return out: ch = ch and $1f gosub clock return clock: wait_amount = 20 gosub wait pokeb portb, ch or $20 gosub wait pokeb portb, ch return PutInter: asm ldd var004 lsrd std var003 addd #var002 xgdx clra ldab 0,x std var008 * lm2i2.bas(63): j = Peekb(addr(Inter)+InterNeeded) * lm2i2.bas(64): if InterLoc and $0001 = 0 ldd #1 anda var004 andb var004+1 cpd #0 beq *+5 jmp _PIIF ldd var006 lsld lsld lsld lsld std var005 * lm2i2.bas(68): InterVar = LShft(InterVar) ldd #240 anda var005 andb var005+1 std var005 * lm2i2.bas(69): InterVar = InterVar and $00F0 ldd #15 anda var008 andb var008+1 std var008 * lm2i2.bas(70): j = j and $0F * lm2i2.bas(71): else jmp _PIEND _PIIF ldd #15 anda var006 andb var006+1 std var005 * lm2i2.bas(72): InterVar = InterValue and $000F ldd #240 anda var008 andb var008+1 std var008 * lm2i2.bas(73): j = j and $F0 * lm2i2.bas(74): endif _PIEND * lm2i2.bas(75): pokeb addr(Inter)+InterNeeded,j or InterVar ldd var003 addd #var002 pshb psha ldd var005 oraa var008 orab var008+1 pulx stab 0,x endasm return GetInter: asm ldd var004 lsrd addd #var002 xgdx clra ldab 0,x std var005 * lm2i2.bas(80): InterVar = peekb(Addr(Inter)+InterNeeded) * lm2i2.bas(81): if InterLoc and $0001 = 0 ldd #1 anda var004 andb var004+1 cpd #0 beq *+5 jmp _GIIF ldd var005 lsrd lsrd lsrd lsrd std var005 * lm2i2.bas(85): InterVar = RShft(InterVar) * lm2i2.bas(86): endif _GIIF ldd #15 anda var005 andb var005+1 std var006 * lm2i2.bas(87): InterValue = InterVar and $000F endasm return InitStr: Datab $02, $02, $0c, $00, $08, $00, $01, $00, $06, $00, $0f