'******************************************************** ' running-bug.bas ' exploration and obastacle avoidance with running gait ' PicBasic Pro Compiler '******************************************************** trisa = %11111111 ' set porta to inputs trisb = %00001100 ' set portb pins 2 & 3 to inputs m_servo var byte ' initialize variables l_servo var byte r_servo var byte temp var byte timer var byte decision var word ir_left var byte ir_right var byte l_count var byte r_count var byte l_threshold var byte r_threshold var byte num_samples var byte behavior var byte num_samples = 30 ' number of samples taken per i.r. module l_threshold = 5 ' noise floor threshold for left detector r_threshold = 5 ' noise floor threshold for right detector for temp = 1 to 10 ' delay for approximately 1 second by sound portb.4,[90 + (temp*3),2] ' flashing LED's and making sounds so that toggle portb.1 ' the infrared detecor board has time to toggle portb.0 ' stabalize on power up. pause 100 next temp start: if portA.2 = 1 then ir_cal gosub infrared ' get sensor values from infrared subroutine portb.1 = ir_left portb.0 = ir_right behavior = ir_left * 2 + ir_right branch behavior,[walk_forward, turn_left, turn_right, walk_reverse] walk_reverse: sound portb.4,[90,1,80,2,125,1,90,2,100,2] for temp = 1 to 2 m_servo = 170 l_servo = 120 r_servo = 120 gosub servo m_servo = 100 l_servo = 160 r_servo = 160 gosub servo next temp random decision if decision.0 = 1 then turn_right turn_left: sound portb.4,[140,1,80,2,125,1,95,2] for temp = 1 to 2 m_servo = 170 l_servo = 120 r_servo = 160 gosub servo m_servo = 100 l_servo = 160 r_servo = 120 gosub servo next temp goto start turn_right: sound portb.4,[140,1,120,2,110,1,100,2] for temp = 1 to 2 m_servo = 170 l_servo = 160 r_servo = 120 gosub servo m_servo = 100 l_servo = 120 r_servo = 160 gosub servo next temp goto start walk_forward: m_servo = 170 l_servo = 160 r_servo = 160 gosub servo m_servo = 100 l_servo = 120 r_servo = 120 gosub servo goto start infrared: l_count = 0 r_count = 0 ir_left = 0 ir_right = 0 for temp = 1 to num_samples ' take 30 samples per detector if portb.2 = 0 then ' if left module senses an object l_count = l_count + 1 ' increase the left count endif if portb.3 = 0 then ' if right module senses an object r_count = r_count + 1 ' increase the right count endif next if l_count >= l_threshold then ' if left count is greater than the threshold ir_left = 1 ' then an object was sensed with left detector endif if r_count >= r_threshold then ' if right count is greater than the threshold ir_right = 1 ' then an object was sensed with right detector endif return ' return to main program servo: ' subroutine to set servos for timer = 1 to 10 pulsout portb.7,m_servo pulsout portb.6,l_servo pulsout portb.5,r_servo pause 13 next timer return ir_cal: ' subroutine to calibrate I.R. sensors low portb.0 If portb.3 = 0 then ' Check right i.r. module output for logic 0 high portb.0 ' turn on right light emitting diode else low portb.0 endif low portb.1 If portb.2 = 0 then ' Check left i.r. module output for logic 0 high portb.1 ' turn on left light emitting diode else low portb.1 endif goto start end