'-------------------------------------------------------------- ' file: calirpd.BAS ' DLC 7/9/2003 ' Wandering object avoider demo code '-------------------------------------------------------------- Dim A As Byte Config Portb = &B10101111 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 Pavoid = 3 'Avoid obstacles 'Declare subs, which are behaviors Declare Sub Doseek Dim Sstate As Byte 'not used yet Dim Sdur As Byte 'not used yet Declare Sub Doavoid Dim Astate As Byte 'Not using state Dim Adur As Byte 'duration of turn Dim Lastturn As Bit 'last turn done Const Tright = 0 Const Tleft = 1 Dim Nextleft As Integer 'second motion Dim Nextright As Integer 'second motion Dim Nextdur As Byte 'second duration Const Turndur = 50 Const Backdur = 50 'typical durations Const Spindur = 80 'for rotate '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 some common speeds, forward and reverse (backwards) Const Fmed = 8 Const Ffast = 12 Const Fslow = 0 Const Bmed = -8 Const Bfast = -12 Const Bslow = 0 '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 = 0 Lmotor = 0 '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! Bpriority = 0 Waitms 5000 Do Call Doseek 'do seek behavior 'Call Doavoid 'do avoid behavior Call Setmotors(tmotorl , Tmotorr) 'set winner Loop End Sub Doseek() 'Just go forward for now at medium speed Getirpd 'Waitms 50 Select Case Rhits Case 20 To 40: Tmotorr = 2 Case 41 To 60: Tmotorr = 4 Case 61 To 80: Tmotorr = 6 Case 81 To 100 Tmotorr = 8 Case 101 To 120: Tmotorr = 12 Case 121 To 140: Tmotorr = 16 Case 141 To 150: Tmotorr = 20 Case Else: Tmotorr = 0 End Select Select Case Lhits Case 20 To 40: Tmotorl = 2 Case 41 To 60: Tmotorl = 4 Case 61 To 80: Tmotorl = 6 Case 81 To 100 Tmotorl = 8 Case 101 To 120: Tmotorl = 12 Case 121 To 140: Tmotorl = 16 Case 141 To 150: Tmotorl = 20 Case Else: Tmotorl = 0 End Select Doseek_done: End Sub Doseek Sub Doavoid() '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 > Pavoid Then Goto Irpd_done 'higher power on 'take priority control Bpriority = Pavoid '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 Astate = 1 'set next state Select Case Irpd Case 0 'Do nothing 'Bpriority = 0 Case 1 : 'the right side Nextleft = Fslow Nextright = Ffast Nextdur = Turndur Tmotorr = Bmed Tmotorl = Bmed Adur = Backdur Case 2 : 'the left side Nextleft = Ffast Nextright = Fslow Nextdur = Turndur Tmotorr = Bmed Tmotorl = Bmed Adur = Backdur Case 3 : 'both, dead ahead If Lastturn = Tleft Then 'turn right now Nextright = Ffast Nextleft = Bfast Lastturn = Tright Else 'turn left now Nextright = Bfast Nextleft = Ffast Lastturn = Tleft End If Nextdur = Spindur Tmotorr = Bmed Tmotorl = Bmed Adur = Backdur '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 Doavoid 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 'end program