' {$STAMP BS1} ' PROGRAM: contest4.bas ' Copyright Ken Boone ' http://kensrobots.com ' kenboone@aol.com ' Program to operate Basic Stomp Erector Set walking robot in the ' Triangle Amateur Robotics club Basic Stomp contest. Contest description ' on web page listed above. ' Robot has two floor sensor micro switches, a front bumper microswitch, ' battery charge sensor, Radio Shack modulated IR sensor and three ' RC servos for localmotion. One servo lifts the outer shell, one servo ' moves the outer shell forward and backwards and the third servo turns ' the robot when it is standing on it's inner foot. Uses the origonal ' Parallex Basic Stamp microcontroller. ' April 6,1996 Comments 10/3/1999 ' March 1,2009 Added {$STAMP BS1} to first line for Stamp Editor ' Stamp I/O pins ******************************************************* ' 0 vertical lift servo - moves outer shell up and down ' 1 horzontal move servo - moves outer shell forwand and back ' 2 turn servo - turns outer shell ' 3 front bumper and floor detector switches ' 4 becon detector ' 5 charging monitor - checks for attachment to charger ' 6 battery monitor - checks battery charge level ' 7 not used ' Constants ************************************************************* SYMBOL vert = 0 ' Output port for vertical servo. SYMBOL horz = 1 ' Output port for horzontal servo. SYMBOL turn = 2 ' Output port for turning servo. SYMBOL front = 120 ' Position front. SYMBOL back = 190 ' Position back. SYMBOL up = 220 ' Position up. SYMBOL down = 140 ' Position down. SYMBOL right = 100 ' Position turned right. SYMBOL left = 200 ' Position turned left. SYMBOL servo = B8 ' Servo to be pulsed. SYMBOL position= B10 ' Pulse length variable SYMBOL i = B2 ' Counter SYMBOL speed = B7 ' Robot speed (servo delay between pulses) ' Set up the Stamp's I/O lines ****************************************** begin: LET PINS = 0 ' Clear the output lines LET DIRS = %00000111 ' 5 inputs, 3 outputs. LET i = right ' Sets up turn counter. again: LET B12 = B12 + 4 ' Keep adding till it rolls over. LET speed = B12 / 5 + 10 ' Speed calculation. LET servo = horz ' Set horzontal servo forward. LET position = front GOSUB m_servo ' Move servo. LET servo = vert ' Set vertical servo up. LET position = up GOSUB m_servo ' Move servo. LET servo = horz ' Set horzontal servo backwards. LET position = back GOSUB m_servo ' Move servo. m_down: LET servo = vert ' Move vertical servo down. LET position = down GOSUB m_servo ' Move servo. INPUT 3 ' Check switch to see if off of table. IF PIN3 = 1 THEN b_turn ' Go to backup and turn routine. r_bat: IF PIN6 = 1 THEN lo_bat ' If battery is low goto low battery. IF PIN5 = 0 THEN again ' If battery is OK goto again. ' Just finished charging or ran into charger GOTO b_turn ' Back up and turn. lo_bat: IF PIN5 = 1 THEN r_bat ' If charge is high goto read battery. ' Need charge find charger. LET B12 = 20 ' Set robot to medium fast speed. LET servo = turn ' Move turn servo to right of travel. LET position = right GOSUB m_servo ' Move servo. LET servo = vert ' Move vertical servo up. LET position = up GOSUB m_servo ' Move servo. r_becon:IF PIN4 = 1 THEN t_right' Don't see becon turn right. ' See becon down then again. LET servo = vert ' Move vertical servo down. LET position = down GOSUB m_servo ' Move servo. GOTO again ' Moves forward one step. t_right:IF i = left THEN t_again' All the way left turn again. ' Don't see becon keep turning. LET i = i + 1 ' Incroment turn position. LET position = i ' New turn position. PULSOUT turn,position ' Turn to new position. PAUSE 20 ' Wait 1/50th second. GOTO r_becon ' Read Becon again. t_again:LET i = right ' Reset counter to right. GOTO m_down ' Turn again. ' Back up and turn routine b_turn: LET servo = turn ' Move turn servo to right of travel. LET position = right gosub m_servo ' Move servo. let servo = vert ' Move vertical servo up. let position = up gosub m_servo ' Move servo. let servo = horz ' Move horzontal servo forwards. let position = front gosub m_servo ' Move servo. let servo = turn ' Move turn servo to the left. let position = left gosub m_servo ' Move servo. goto m_down ' Go back to forward motion section. ' Servo control subroutine. m_servo:for b2 = 0 to 20 ' Loop 20 times. pulsout servo,position ' Send servo pulse. pause speed ' Pause between pulses (varies) next return