' -----[ Title ]----------------------------------------------------------- ' ' Date...... 12 Jul 2001 ' ' File...... bs2dist.txt ' Purpose... Robot with distance sensors, Basic Stamp 2, ' and modified servos for wheels. ' Example code illustrating the method. ' Author.... Jim Ubersetzig ' jim.ubersetzig@lmco.com ' Started... 2 Jul 2001 ' -----[ Revision History ]------------------------------------------------ ' ' ' 12 Jul 01 Simplified from Jim Ubersetzig's firefighting ' robot software. ' -----[ General Description ]--------------------------------------------- ' ' The software is for a robot that uses distance measuring sensors to control ' it's motion. It has a Basic Stamp 2 computer, GP2D02 distance sensors, ' and modified servos for the wheels. ' ' The software uses 50 % of the RAM and 31 % of the flash memory ' in the Basic Stamp 2 computer. ' Software functions are provided for these behaviors : ' ' Go_Forward ' Go straight forward. Distance is specified in variable trip_dist. ' ' Follow_Wall_Forward ' Follow the right hand wall while rolling forward. ' Stop if a wall is directly ahead. ' Stop if the right wall ends. ' ' R90_align ' Turn right 90 degrees then align with the right hand wall. ' ' L90_align ' Turn left 90 degrees then align with the right hand wall. ' ' R90 ' Turn right. ' ' Follow_Wall_Backward ' Backup following the right hand wall. ' Terminate when there is no wall to the right. ' Else pivot clockwise at each corner and continue backing up.' ' ' Go_Backward ' Go straight backwards. Distance is specified in variable trip_dist. ' ' -----[ Detailed Description ]-------------------------------------------- ' ' ' range(front) ' . ' . ' . ________________ ' / \ ' / . ,-, \ ' / . | | \ ' / . | > . . . . . \ . . . . range(right_front) ' | . | | | ' | . '-' | ' \_ . _/ ' . ' ,---, . ,---, ' | | | | ' | W | ._/\_. | W | ' | H | | | | H | ' | E | -------- TOP VIEW | E | ' | E | | | | E | ' | L | '-\/-' | L | ' | | . | | ' '---' . '---' ' _ . _ ' / . \ ' | . ,-, | ' | . | | | ' \ . | > . . . . . / . . . . . range(right_rear) ' \ . | | / ' \ . '-' / ' \__________________/ ' . ' . ' . ' range(rear) ' ' ' Note that four sensors are mounted on the robot. If the robot were traveling ' along a wall, the sensors would report distance from the right wall in 2 places, ' and also report distance to anything the robot is about to run into front or back ' of the robot. ' ' The raw sensor data is reported as a number in the range 225 to 70, which ' corresponds to distances from 4 inches to 3 feet. This is converted in software ' to distance reported in tenths of an inch measured from the robot center. ' ' Distances greater than 3 feet are reported as three feet. ' ' The sensor is highly tolerant of room lighting (except sunlight) ' and is not affected by colors or reflectances of target objects. ' ' ' ,-------, ' | ,---, | ' | | O | | ' yellow wire --------------|| | | | ' red wire --------------|| | | | GP2D02 distance sensor ' green wire --------------|| '___' | Wire Colors ' black wire --------------|| | ' | | ' | O | ' '-------' ' -----[ Basic Stamp 2 Computer Wiring ]----------------------------------------------- ' 'front PIN 0 ' Diode to GP2D02 sensor clock input. ' Anode to sensor green wire. 'right_front PIN 1 ' Diode to GP2D02 sensor clock input. ' Anode to sensor green wire. 'right_rear PIN 2 ' Diode to GP2D02 sensor clock input. ' Anode to sensor green wire. 'rear PIN 3 ' Diode to GP2D02 sensor clock input. ' Anode to sensor green wire. Pin_ir_Data CON 7 ' 4.7K resistor to junction of: ' 47K resistor to +5VDC, ' diodes to each GP2D02 sensor ' data output. Cathodes to yellow ' wires of the sensors. Right_Wheel CON 8 ' 330 ohm resistor to white wire of modified servo. Left_Wheel CON 9 ' 330 ohm resistor to white wire of modified servo. ' -----[ Variables and Constants ]----------------------------------------- ' i VAR byte ' time tic counter (approx 600 Hz) servo_time CON 8 ' time between pulsing the servos (in time tics) ' 8 yields 55 Hz pulse rate to servos. sensor_time CON 32 ' time between using a range sensor (in time tics) ' 32 yields 110 Hz (frequency counter) ' = approx. 110/9 = 82 mS per measurement. drive VAR byte ' requested speed (+ for forward) forward CON 45 backup CON -45 none CON 0 steer VAR byte ' requested steering (+ for right) straight CON 0 pivot_left CON -21 pivot_right CON 20 offset_r CON 725 ' to stop the wheels offset_l CON 755 integral var byte mask VAR nib ' field of bits which activate distance sensors. ' must be non-zero to avoid infinite loop. measure_front CON %0001 measure_right CON %0110 ' typical mask values measure_rear CON %1000 range VAR byte(4) ' raw data from distance sensors j VAR nib ' selected distance sensor range 0..3 front CON 0 right_front CON 1 ' indexes for the range array right_rear CON 2 ' = values for j rear CON 3 hall_center CON 200 ' vehicle is centered in the hallway. ' 17 nov 00 jru front_wall CON 125 ' wall is 9 inches ahead on vehicle center. rear_wall CON 125 ' wall is 9 inches behind vehicle center. ' 11 jul 00 no_wall CON 220 ' too far away to be a corridor wall. ' PID constants K1 CON 80 ' backing up proportional K2 CON 140 ' backimg up rate term K3 CON 1 ' not used K4 CON 1 ' pivot to match wall proportional term K5 CON 80 ' forward motion proportional K6 CON 140 ' forward motion rate term temp VAR word trip_dist VAR byte pivot_time CON 27 ' -----[ Initialization ]------------------------------------------------------- ' Note: upon electrical power application, all variables are forced to zero, ' all I/O pins are forced to be inputs to the computer. for i = 0 to 3 ' distance sensors need to be prepared for use. high i next LOW Right_Wheel ' propulsion motors are modified servos LOW Left_Wheel mask = measure_front PAUSE 5000 ' wait 5 seconds ' Test code - drive straight forward while reporting all the distance measurements ' This is a good test to make sure everything is working, note that is commented out ' (every line starts with a ' ) Remove the ' to use this test code, then put the ' s back. ' drive = 20 ' steer = 0 ' mask = measure_front+measure_right+measure_rear 'loop: gosub doit ' debug dec ? range(front), dec ? range(right_front), dec ? range(right_rear), dec ? range(rear),cr ' goto loop ' -----[ Main Code ]------------------------------------------------------- ' WALL ' ,---------------------------------------, ' | 2 <----- 1 5 -------------> | ' | | | ' | 3 v | ' ----' '------ ' 4 6 main: gosub follow_wall_forward ' 1 gosub l90_align ' 2 gosub follow_wall_forward ' 3 trip_dist = 28 gosub go_backward ' 4 gosub align gosub follow_wall_backward ' 5 trip_dist = 28 gosub go_forward ' 6 gosub align gosub follow_wall_forward gosub l90_align goto main ' -----[ Simple Behaviors ]----------------------------------------------------- ' Input is trip_dist(ance) required. Move straight forward the requested distance. ' If trip_dist = 100, then it travels 52 inches straight forward. go_forward: drive = forward steer = straight mask = measure_front forward1: gosub doit: if trip_dist > 0 then forward1 return ' Backup following the right hand wall. ' Terminate when there is no wall to the right. ' Else pivot clockwise at each corner and continue backing up. btnw10: gosub r90_align ' Move backward until the right wall disappears. ' Maintain correct distance from the right wall. follow_wall_backward: drive = backup steer = straight mask = measure_right + measure_rear ' 12 jul 2000 jru btnw1: gosub doit ' now calculate the PID steering solution steer = (range(right_front)+range(right_rear)-hall_center) */ K1 'proportional term steer = -steer steer = (range(right_front)-range(right_rear)) */ K2 + steer ' rate term ' There is a problem if the vehicle is at too sharp an angle ' with the wall. The following code fixes the problem (especially ' evident when the distance to the wall exceeds 11 inches). if (range(right_front)-range(right_rear)) < 10 then btnw4 if (range(right_rear)-range(right_front)) < 10 then btnw4 if (range(right_front)-range(right_rear)) > $3fff then btnw3 steer = 10: goto btnw4 btnw3: steer = -10 btnw4: if range(rear) < rear_wall then btnw10 ' 12 jul 2000 jru if (range(right_front) > 120) OR (range(right_rear) < 140) then btnw1 trip_dist = 26 ' input is trip_dist(ance) required. Move straight backward the requested distance. ' if trip_dist = 100, it rolls back 48 inches. go_backward: drive = backup steer = straight mask = measure_rear backward1: gosub doit: if trip_dist > 0 then backward1 return ' Turn right 90 degrees then align with the right hand wall. r90_align: gosub r90 align: drive = none steer = straight mask = measure_right integral = 0 align1: gosub doit ' now calculate the PID steering solution temp = (range(right_front)-range(right_rear))*K4 ' proportional term steer = temp: if abs(temp) > 5 then align1 integral = integral + steer: steer = integral if abs(temp) > 1 then align1 return ' Turn left 90 degrees then align with the right hand wall. l90_align: drive = none steer = pivot_left mask = measure_front trip_dist = pivot_time la1: gosub doit if trip_dist > 0 then la1: goto align ' Move forward until vehicle might hit a something ahead, ' also exit when the right wall dissapears. ' Maintain correct distance from the right wall. follow_wall_forward: drive = forward steer = straight mask = measure_right + measure_front fthonw1: gosub doit ' now calculate the PID steering solution steer = (range(right_front)+range(right_rear)-hall_center) */ K5 'proportional term steer = (range(right_front)-range(right_rear)) */ K6 + steer ' rate term ' There is a problem if the vehicle is at too sharp an angle ' with the wall. The following code fixes the problem (especially ' evident when the distance to the wall exceeds 11 inches). if (range(right_front)-range(right_rear)) < 20 then fthonw4 if (range(right_rear)-range(right_front)) < 20 then fthonw4 if (range(right_front)-range(right_rear)) > $3fff then fthonw3 steer = 10: goto fthonw4 fthonw3: steer = -10 fthonw4: if range(front) < front_wall then fthonw5 if (range(right_rear) > 120) OR (range(right_front) < 140) then fthonw1 fthonw5: return ' Turn right 90 degrees (approximately). r90: drive = none steer = pivot_right mask = measure_front trip_dist = pivot_time r1: gosub doit if trip_dist > 0 then r1: return '--------------------------------------------------------------------------- ' This gets called from anywhere and functions to keep the wheel servos and ' distance sensors happy. It returns periodically when it is time to evaluate ' the distance sensor data and make corrections to the motor speeds. You should ' evaluate completion criterion before calling again. doit: if mask.bit0(j) = 0 then doit3 ' but skip if it is masked off low j ' prep the next sensor pause 1 ' not sure the pause is needed doit1: i=i+1 if i//servo_time <> 0 then doit2 pulsout Right_Wheel, steer.bit7*$ff00+steer-(drive.bit7*$ff00+drive)+offset_r pulsout Left_Wheel, steer.bit7*$ff00+steer+(drive.bit7*$ff00+drive)+offset_l doit2: if i//sensor_time <> 0 then doit1 ' ready to clock in the ir dist sensor you most recently prepped. SHIFTIN Pin_ir_Data,j,msbpost,[range(j)] ' Then shift in the data high j ' deselect the sensor read j*3,temp.highbyte: read j*3+1,temp.lowbyte range(j) = temp / range(j) max 295: read j*3+2, temp.lowbyte range(j) = range(j) - temp.lowbyte ' convert to tenths of inch doit3: j=j+1//4 ' select the next sensor if j <> 0 then doit trip_dist=trip_dist-1 return ' -----[ Calibration of Distance Sensors ]----------------------------------- ' This software calculates the distance in tenths of an inch from the ' range numbers ( 0 .. 255 ) reported by GP2D02 distance semsors. ' K1 ' The equation used is tenths = ------ - K2 ' range ' ' The first two numbers in each row are K1.highbyte and K1.lowbyte ' The last number in each row is K2 data 17029/256, 17029, 45 ' front distance sensor data 21453/256, 21453, 59 ' right_front data 20365/256, 20365, 58 ' right_rear data 16591/256, 16591, 42 ' rear distance sensor ' THE END