'******************************************************** '******************************************************** 'Cricket 6 Legged Robot Program for STAMP II SX 'NOTE: All Timing has changed for SX implementation 'Complete Walking Program with sound and blinking LED 'USES SONY REMOTE - this program suppports the Sony Remote 'decoder connected to pin 10. This decoder converts 12 bit 'Sony IR communication to 8 bit ASCII strings which can be 'read using "serin" command in BS2 language. 'May 1999 Henry Arnold 'Added more sounds and actions. Improved Gosub returns Rev 3.0 'Added infrared transmit using 555 h/w and IR LED. 'Revision 5.10 must be used with rev 3.0 Sony.c Decoder S/W 'fixed Sony decoder problems. '******************************************************** '******************************************************** '************* Start up Initialization Code ************* 'The following constants & variables are in use by this program 'servo min must not be less than 1250 and max must not be more than 2500 'this is important so as not to damage servos. Each unit = 0.8us max_pos_r con 2125 'max position mid_pos_r con 1875 'neutral position min_pos_r con 1625 'min position max_pos_c con 2250 'max position mid_pos_c con 1875 'neutral position min_pos_c con 1375 'min position max_pos_l con 2450 'max position mid_pos_l con 1875 'neutral position min_pos_l con 1275 'min position right_motor_pos var word 'right servo position left_motor_pos var word 'left servo position center_motor_pos var word 'center servo position left_motor con 0 'left motor connected to pin 0 center_motor con 1 'center motor connected to pin 1 right_motor con 2 'right motor connected to pin 2 r_feeler con 3 'right feeler l_feeler con 4 'left feeler eyes con 5 'green eye LEDs speaker con 6 'speaker on pin 6 of stamp IR_in con 7 'Sharp IR module in IR_out con 8 '555 IR LED Control LED con 9 'Red LED for flashing Sony_in con 10 'Sony remote code serial in xservo1 con 11 'extra servo port xservo2 con 12 'extra servo port xservo3 con 13 'extra servo port xservo4 con 14 'extra servo port xservo5 con 15 'extra servo port pulses con 17 'number of pulses for each motor cmd delay con 20 'pause between pulses and ir timeout in ms mode_r con 1021 'ir serial port mode = 2400 baud mode_t con 18447 'ir transmit port mode = 1200 baud right_feeler var IN3 'right feeler is connected to pin 3 left_feeler var IN4 'left feeler is connected to pin 4 ncount var byte 'loop count beep var word 'for sound mcount var byte 'other loop count ir_data var byte 'infrared input character 'PROGRAM STARTS EXECUTION HERE************************** '********Feeler Input Setup***************************** input r_feeler 'make pin 3 right feeler an input pin input l_feeler 'make pin 4 left feeler an input pin '*******Starting sound snippet******************* gosub sound0 'this makes a bunch of sounds center_motor_pos = max_pos_c 'Center motors left_motor_pos = mid_pos_l gosub walkone center_motor_pos = min_pos_c 'Center motors right_motor_pos = mid_pos_r gosub walkone center_motor_pos = mid_pos_c 'Center motors gosub walkone serout IR_out,mode_t,10,[cr,cr,"*****************************",cr] serout IR_out,mode_t,10,["* This is Cricket the Robot *",cr] serout IR_out,mode_t,10,["*****************************",cr] pause 1000 'wait 1 second before beginning '****************** Start of Main Loop ****************** start: low IR_out 'shut off IR output '(not used in this prog) low eyes 'turn eyes on - active low center_motor_pos = max_pos_c 'Take a step forward right_motor_pos = max_pos_r 'using right side left_motor_pos = max_pos_l gosub walkone if left_feeler = 0 then left 'if left feeler if right_feeler = 0 then right 'if right feeler center_motor_pos = min_pos_c 'Take another step right_motor_pos = min_pos_r 'using left side left_motor_pos = min_pos_l gosub walkone if left_feeler = 0 then left 'if left feeler if right_feeler = 0 then right 'if right feeler goto start 'do it all again '******************************************************** '***** got right feeler hit ***************************** '******************************************************** 'Cricket needs to step backward three steps and 'then turn to the left to avoid obstacle hitting right 'feeler right: gosub sound9 gosub backup 'backup a little 'now go left for ncount=1 to 3 'Do a left turn center_motor_pos = max_pos_c 'right side up right_motor_pos = max_pos_r left_motor_pos = min_pos_l gosub walkone center_motor_pos = min_pos_c 'right side down right_motor_pos = min_pos_r left_motor_pos = max_pos_l gosub walkone next goto start 'keep walking '******************************************************** '***** got left feeler hit ****************************** '******************************************************** 'Cricket needs to step backward three steps and 'then turn to the right to avoid obstacle hitting left 'feeler left: gosub sound3 gosub sound3 gosub sound3 gosub backup 'backup a little 'now go right for ncount=1 to 3 center_motor_pos = min_pos_c 'right side down right_motor_pos = max_pos_r left_motor_pos = min_pos_l gosub walkone center_motor_pos = max_pos_c 'right side up right_motor_pos = min_pos_r left_motor_pos = max_pos_l gosub walkone next goto start '******************** SUBROUTINES *********************** '******************************************************** '******** Walk one cycle ******************************** '******************************************************** 'This routine executes all the walk operations by 'moving the motors to the asked for positions. 'Infrared commands are looked for after each move. 'It also makes a beep sometimes by looking at the 'result of a random number query. walkone: Random beep 'make a sound if beep < 58000 then no_sound 'frequency based on random if beep < 62000 then beeps 'frequency based on random if beep < 65000 then beeps1 'frequency based on random gosub sound9 'special ocassional sound goto no_sound beeps1: gosub sound5 'special ocassional sound goto no_sound beeps: freqout speaker,150,2080 'number freqout speaker,75,1680 high eyes 'turn eye LEDs off - active low no_sound: toggle LED 'blinks red LED for mcount=1 to pulses '# of pulses pulsout center_motor,center_motor_pos 'do side gosub check_ir next for mcount=1 to pulses pulsout right_motor,right_motor_pos 'right leg position pulsout left_motor,left_motor_pos 'left leg position pulsout center_motor,center_motor_pos 'centerleg position gosub check_ir next return '******************************************************** '******** BACKUP **************************************** '******************************************************** 'Take three steps backwards backup: for ncount=1 to 3 center_motor_pos = min_pos_c 'right side down right_motor_pos = max_pos_r left_motor_pos = max_pos_l gosub walkone center_motor_pos = max_pos_c 'right side up right_motor_pos = min_pos_r left_motor_pos = min_pos_l gosub walkone next return '******************************************************* 'This routine is same as backup except the IR input is ignored 'while this is executed. back_it_up: center_motor_pos = min_pos_c 'right side down right_motor_pos = max_pos_r left_motor_pos = max_pos_l gosub walk center_motor_pos = max_pos_c 'right side up right_motor_pos = min_pos_r left_motor_pos = min_pos_l gosub walk goto check_ir '******************************************************* center_legs: center_motor_pos = max_pos_c 'Center motors left_motor_pos = mid_pos_l gosub walk center_motor_pos = min_pos_c 'Center motors right_motor_pos = mid_pos_r gosub walk center_motor_pos = mid_pos_c 'Center motors gosub walk low eyes goto stop_it '******************************************************* forward_fast: center_motor_pos = max_pos_c 'right side down right_motor_pos = max_pos_r left_motor_pos = max_pos_l gosub walk center_motor_pos = min_pos_c 'right side up right_motor_pos = min_pos_r left_motor_pos = min_pos_l gosub walk goto check_ir '******************************************************* right_turn: center_motor_pos = max_pos_c 'right side up right_motor_pos = max_pos_r left_motor_pos = min_pos_l gosub walk center_motor_pos = min_pos_c 'right side down right_motor_pos = min_pos_r left_motor_pos = max_pos_l gosub walk goto check_ir 'keep walking '******************************************************** left_turn: center_motor_pos = min_pos_c 'right side down right_motor_pos = max_pos_r left_motor_pos = min_pos_l gosub walk center_motor_pos = max_pos_c 'right side up right_motor_pos = min_pos_r left_motor_pos = max_pos_l gosub walk goto check_ir '******************************************************** walk: toggle LED 'blinks LED for mcount=1 to 15 '# of pulses pulsout center_motor,center_motor_pos 'do side pause 8 next for mcount=1 to 15 pulsout right_motor,right_motor_pos 'right leg position pulsout left_motor,left_motor_pos 'left leg position pulsout center_motor,center_motor_pos 'centerleg position pause 8 next return '******************************************************** check_ir serin Sony_in,mode_r,delay,pass, [ir_data] 'read ir port ' debug dec? ir_data 'used for debugging only if ir_data = 92 then left 'check for valid commands if ir_data = 91 then right if ir_data = 144 then forward_fast if ir_data = 145 then back_it_up if ir_data = 147 then right_turn if ir_data = 146 then left_turn if ir_data = 137 then sound0 if ir_data = 128 then sound1 if ir_data = 129 then sound2 if ir_data = 130 then sound3 if ir_data = 131 then sound4 if ir_data = 132 then sound5 if ir_data = 133 then sound6 if ir_data = 134 then sound7 if ir_data = 135 then sound8 if ir_data = 136 then sound9 if ir_data = 88 then center_legs if ir_data = 89 then stop_it pass: return '******************************************************** stop_it: serin Sony_in,mode_r,delay,stop_it,[ir_data] 'wait for command 'Keep waiting till valid remote key is hit and then check_ir again if ir_data = 144 then check_ir if ir_data = 145 then check_ir if ir_data = 146 then check_ir if ir_data = 147 then check_ir if ir_data = 91 then check_ir if ir_data = 92 then check_ir if ir_data = 137 then check_ir if ir_data = 128 then check_ir if ir_data = 129 then check_ir if ir_data = 130 then check_ir if ir_data = 131 then check_ir if ir_data = 132 then check_ir if ir_data = 133 then check_ir if ir_data = 135 then check_ir if ir_data = 136 then check_ir if ir_data = 88 then check_ir if ir_data = 89 then check_ir goto stop_it '******************************************************** sound0: freqout speaker,350,1680 'this makes a bunch of sounds freqout speaker,225,1440 freqout speaker,225,1520 'ALREADY CONVERTED freqout speaker,75,1320 freqout speaker,75,1120 freqout speaker,75,960 freqout speaker,350,1680 freqout speaker,225,800 freqout speaker,175,1680 goto check_ir sound1: freqout speaker,100,1760 freqout speaker,50,1440 freqout speaker,150,1400 freqout speaker,75,1680 freqout speaker,25,1520 freqout speaker,225,1200 'ALREADY CONVERTED freqout speaker,25,960 freqout speaker,100,1720 freqout speaker,125,1520 freqout speaker,50,880 freqout speaker,375,0 goto check_ir sound2: freqout speaker,250,1120 freqout speaker,500,960 'ALREADY CONVERTED freqout speaker,350,1680 freqout speaker,75,800 goto check_ir sound3: freqout speaker,150,2080,1600 freqout speaker,75,1680 freqout speaker,150,2080,1600 'ALREADY CONVERTED freqout speaker,75,1680 freqout speaker,250,0 goto check_ir sound4: freqout speaker,750,720 freqout speaker,1500,480 'ALREADY CONVERTED freqout speaker,1250,400 goto check_ir sound5: freqout speaker,50,880 freqout speaker,50,960 freqout speaker,50,1040 freqout speaker,50,1120 freqout speaker,50,1760 freqout speaker,50,1600 freqout speaker,50,1440 'ALREADY CONVERTED freqout speaker,50,1360 freqout speaker,50,1200 freqout speaker,50,1120 goto check_ir sound6: freqout speaker,500,1280 goto check_ir sound7: freqout speaker,500,1480 goto check_ir sound8: freqout speaker,500,1680 goto check_ir sound9: gosub chirp gosub chirp freqout speaker,375,0 for ncount = 1 to 4 gosub chirp next goto check_ir chirp: freqout speaker,35,1800 freqout speaker,27,1680 freqout speaker,35,1800 freqout speaker,27,1680 freqout speaker,35,1800 freqout speaker,27,1680 freqout speaker,35,1800 freqout speaker,27,1680 freqout speaker,375,0 return '********************************************************