'-------------------------------------------------------------- ' file: minisumo.BAS ' DLC 7/7/2003 ' Sumo demo code '-------------------------------------------------------------- Dim A As Byte Config Portb = &B10101110 Config Portd = &B11111000 'Port B pins 'B7 = SCK output 'B6 = MISO input 'B5 = MOSI output 'B4 = IRM input 'B3 = 40KHz IR modulation output 'B2 = Left IR LED enable output 'B1 = Right IR LED enable output 'B0 = LED signal output 'Port D pins 'D7 = N/A set to output 'D6 = Left servo output 'D5 = Right servo output 'D4 = LED signal output 'D3 = unassigned output 'D2 = Left line sensor input 'D1 = Center line sensor input 'D0 = Right line sensor input $crystal = 10000000 'Sets 40KHz OC for IRPD LEDs Config Timer1 = Timer , Prescale = 1 , Compare A = Toggle Set Tccr1b.3 Compare1a = 125 'Behavior priorities Dim Bpriority As Byte 'Behavior priority currently Const Pseek = 1 'just wander Const Pchase = 3 'chase opponent Const Pedge = 5 'edge detect 'Declare subs, which are behaviors Declare Sub Doseek Dim Sstate As Byte 'not used yet Dim Sdur As Byte 'not used yet Declare Sub Dochase Dim Astate As Byte 'Not using state Dim Adur As Byte 'duration of turn Dim Lastturn As Byte 'last turn done Const Tright = 2 Const Tleft = 1 Const Tnone = 0 Dim Nextleft As Integer 'second motion Dim Nextright As Integer 'second motion Dim Nextdur As Byte 'second duration Const Turndur = 20 Const Backdur = 50 'typical durations Const Spindur = 80 'for rotate Const Fwddur = 1 'go forward 'Declare utility subs Declare Sub Getirpd Dim Lhits As Byte Dim Rhits As Byte Dim Lback As Byte Dim Rback As Byte Dim Irpd As Byte Dim Bs As Byte Declare Sub Setmotors(byval Lmot As Integer , Byval Rmot As Integer) Dim Tmpa As Integer Dim Rmotor As Integer Dim Lmotor As Integer Dim Tmotorl As Integer Dim Tmotorr As Integer Declare Sub Getlines Dim Lines As Byte Dim Rlines As Integer Dim Ldir As Byte Dim Dir As Byte Declare Sub Doedge Dim Estate As Byte Dim Edur As Byte 'Declare some common speeds, forward and reverse (backwards) Const Fmed = 8 Const Ffast = 12 Const Fslow = 0 Const Bmed = -8 Const Bfast = -12 Const Bslow = -2 '22.5us per step issued every 43ms 'smaller makes right go forward 'left motor center = 65, right = 64 Config Servos = 2 , Servo1 = Portd.6 , Servo2 = Portd.5 , Reload = 10 'These are good URCP numbers for medium high speed. Rmotor = 10 Lmotor = 10 'Setmotors() uses URCP directions (-32 to + 32) to set motor direction and 'speed. Negative number is backwards, positive is forwards. 0 = stop. Call Setmotors(lmotor , Rmotor) 'set servo initial values points before starting servo interrupts. Portd.5 = 0 Portd.6 = 0 Portb.0 = 0 Portb.1 = 1 Portb.2 = 1 'Wait 5 seconds after power is applied, then... 'Waitms 5000 'Enable the servos now. Enable Interrupts 'start robot behaviors 'Note that all behavior functions are run at each iteration of the loop. The 'behavior with the highest priority is the one that gets control of the 'motors and that priority behavior will release control when its done, allowing 'the lower priority behaviors to run again. Its up to you to decide if you 'want the lower priority behaviors to take up where they left off or to start 'over. There are good arguments either way. I usually have them restart, but 'some are more entertaining if they just pick up where they left off! ' 'The nice thing about breaking all your stuff into behaviors and assigning 'them a priority is that you can add and remove behaviors without modifying 'any other behavior. By deciding how your behaviors will interract with 'each other (interrupt or restarting behaviors) you will get some emergent 'behavior that you didn't necessarily plan for. Sometimes these are good, 'sometimes not! 'Clear all state machines Bpriority = 0 Astate = 0 Estate = 0 'This will start the 5 second timer and avoid the delay if a processor resets. Portd.3 = 1 Portd.4 = 0 If pinb.0 = 1 Then While pinb.0 = 1 Wend Waitms 5000 End If Do Call Doseek 'do seek behavior Call Dochase 'chase behavior Call Doedge 'edge behavior Call Setmotors(tmotorl , Tmotorr) 'set winner Loop End Sub Doseek() 'Just go forward for now at medium speed If Bpriority > Pseek Then Goto Doseek_done 'higher power on Bpriority = Pseek Tmotorl = Fmed Tmotorr = Fmed If Lastturn = Tright Then Tmotorl = Fmed Tmotorr = Bmed Waitms 100 Lastturn = Tnone Elseif Lastturn = Tleft Then Tmotorl = Bmed Tmotorr = Fmed Waitms 100 Lastturn = Tnone End If Doseek_done: End Sub Doseek Sub Doedge() If Bpriority > Pedge Then Goto Edge_done 'higher power on On Estate Goto Estate0 , Edecr , Estate2 , Edecr , Edone Estate0: 'first move Getlines 'get line detector values If Lines = 7 Then Goto Edge_done 'nothing to do 'take priority control Bpriority = Pedge Estate = 1 'set next state Tmotorl = Bfast 'back up quickly Tmotorr = Bfast Edur = Backdur Goto Edge_done Estate2: 'second half of action Tmotorl = Bmed Tmotorr = Fmed Edur = Spindur Estate = 3 'Bpriority = Pchase 'see something, go for it. Goto Edge_done Edecr: 'decr timer 'This processor is so fast that I needed to use actual time wasters, 'looping was not enough! When the loop count is expended the state 'increments. If Edur > 0 Then Decr Edur Waitms 4 Else Incr Estate End If Goto Edge_done Edone: ' finished, go away now Estate = 0 Bpriority = 0 Edge_done: End Sub Doedge Sub Dochase() 'This behavior shows how to implement a modified finite state machine (FSM) 'as a behavior. Avoid is broken into 5 states, each state does its thing and 'then leaves the subroutine so other processing can be done. This allows the 'behavior to be interrupted, or subsumed by a higher priority one. Broken 'down the states are: '0 = look at IRPD and set movements to do, set up first movement. '1 = count down the timer for the first movement. '2 = set the second group of values for the second movement. '3 = count down the timer for the second movement, uses same routine. '4 = finished, clean up and release priority. 'avoid obstacles If Bpriority > Pchase Then Goto Irpd_done 'higher power on 'here on states 1 and 3 we'll go to the timer stage On Astate Goto Astate0 , Adecr , Astate2 , Adecr , Adone Astate0: 'IRPD & first move 'Get the irpd value, Getirpd 'Now select what to do based on what is detected. Note that we set 'the speed of each motor and a duration value. That Adur is used so 'that we keep acting even after the stimulus is gone. In the case of 'being cornered and turning around this is critical. During this state 'we set both the first and second movements, in later states we will 'set the current motor speed and timers to the "next" values for the 'subsequent state's movement. If Irpd = 0 Then Goto Irpd_done 'nothing to do 'take priority control Bpriority = Pchase Astate = 1 'set next state Select Case Irpd Case 0 'Do nothing 'Bpriority = 0 Case 1 : 'the right side Nextleft = Ffast Nextright = Fslow Nextdur = Fwddur Tmotorr = Bslow Tmotorl = Ffast Lastturn = Tright Adur = Turndur Case 2 : 'the left side Nextleft = Ffast Nextright = Ffast Nextdur = Fwddur Tmotorr = Ffast Tmotorl = Bslow Lastturn = Tleft Adur = Turndur Case 3 : 'both, dead ahead Nextright = Ffast Nextleft = Ffast Lastturn = Tnone Nextdur = Fwddur Tmotorr = Ffast Tmotorl = Ffast Adur = Fwddur 'back up for a bit End Select Goto Irpd_done Adecr: 'decr timer 'This processor is so fast that I needed to use actual time wasters, 'looping was not enough! When the loop count is expended the state 'increments. If Adur > 0 Then Decr Adur Waitms 4 Else Incr Astate End If Goto Irpd_done Astate2: 'second move Tmotorr = Nextright Tmotorl = Nextleft Adur = Nextdur Incr Astate 'ready next state Goto Irpd_done Adone: 'finished, clean up Bpriority = 0 'done Astate = 0 Irpd_done: End Sub Dochase Sub Getirpd() 'this routine looks at the IR Proximity detector and returns a value that 'shows if an object is to the left, right or dead ahead. The detection is: '0: Nothing '1: To the right '2: To the left '3: dead ahead ' The routine not only looks to see if there is something 'seen, but also checks the background noise and insists that the detect 'is higher than the noise level before reporting a real detection. 'Currently set to 50, which is too sensitive IMO. Set Portb.1 Set Portb.2 Irpd = 0 Portb.1 = 0 Rhits = 0 Rback = 0 For A = 1 To 150 If Pinb.4 = 0 Then Rhits = Rhits + 1 End If Next A Portb.1 = 1 For A = 1 To 150 If Pinb.4 = 0 Then Rback = Rback + 1 End If Next A Rback = Rback + 70 If Rhits > Rback Then Irpd = 1 End If Waitus 600 Portb.2 = 0 Lhits = 0 Lback = 0 For A = 1 To 150 If Pinb.4 = 0 Then Lhits = Lhits + 1 End If Next A Portb.2 = 1 For A = 1 To 150 If Pinb.4 = 0 Then Lback = Lback + 1 End If Next A Lback = Lback + 70 If Lhits > Lback Then Irpd = Irpd + 2 End If End Sub Getirpd Sub Setmotors(lmot , Rmot) 'uses URCP values -32 to +32 for motor speeds Dim Tlmotor As Byte Dim Trmotor As Byte Rmot = 0 - Rmot 'invert this one 'Lmot = Lmot / 2 'Tmpa = Lmot / 2 'Lmot = Lmot + Tmpa Tlmotor = 97 + Lmot 'lmotor trimmed up one 'Rmot = Rmot / 2 'Tmpa = Rmot / 2 'Rmot = Rmot + Tmpa Trmotor = 97 + Rmot Servo(1) = Tlmotor Servo(2) = Trmotor End Sub Setmotors Sub Getlines() 'Reads in the line sensor values Lines = Pind And &H07 End Sub Getlines 'end program