'{$STAMP BS2p} 'HERBERT.BSP 'Copyright Harry W. Lewis 2002 'BSP routine to drive Libby board for head 'revision 6/24/2002 'Generic values SPKR con 12 'Speaker port address LF con 10 'Line Feed robot_run VAR BIT 'remembers that the robot is running already 'These are for the servo routines - 0 and 1 are the left and right wheels LEFT_RIGHT con 2 'left and right servo port UP_DN con 3 'up and down servo port MOUTH con 4 'mouth servo port EYES con 5 'eye servo port SERVOMIN var byte 'min value for servo = 1ms SERVONOM con 127 'center value for servo = 1.5ms SERVOMAX var byte 'max value for servo = 2ms WAIT_TIME var byte 'value to load timer, 1 count = 20 ms WRITE_IT CON 128 'changes an address from read to a write command RAMP CON 16 'start address of Servo Ramps SETPOINT CON 8 'start address of Servo Setpoints COARSE CON 32 'the bit to change ramp values to coarse mode ' = 480us to 2520us (fine mode = 994us to 2014us) servodrive var byte 'value to pulseout rampLR var byte 'Ramp rate to Left/Right servo rampUD var byte 'Ramp rate to Up/Down servo rampM var byte 'Ramp rate to Mouth servo rampE var byte 'Ramp rate to Eyes servo SerDIn var byte 'holds data back form Co-Processor rampLR = 16 'Ramp rate to Left/Right servo (nom = 16) rampUD = 16 'Ramp rate to Up/Down servo (nom = 16) rampM = 16 'Ramp rate to Mouth servo (nom = 16) rampE = 16 'Ramp rate to Eyes servo (nom = 16) port var bit 'Variable to select state of I/O 'if it is 0 use MAINIO 'if it is 1 use AUXIO port_save VAR BIT 'a place to save port when changing it temporarily channel var nib 'used to select servo channel to be used 'set up for running robot_run = 0 'robot mode is not running AUXIO 'set up for X0-X15 input/outputs SEROUT 10, 1021, [116] SEROUT 10, 1021, [116] 'Reset Co-Processor GOSUB set_ramps 'setup servo ramps on channels for Herbert freqout SPKR,250,500,4000 'send a beep from the on-board speaker '***************************** Main: ' 'Start with all servos in center position ' first byte is 136 + servo channel number ' second byte desired position 0-255 SEROUT 10, 1021, [138,127] 'Set Servo 2 = Left/Right = Center SEROUT 10, 1021, [139,127] 'Set Servo 3 = Up/Down = Center SEROUT 10, 1021, [140,127] 'Set Servo 4 = Mouth = Center SEROUT 10, 1021, [141,127] 'Set Servo 5 = Eyes = Center WAIT_TIME = 50 '50*20ms = 1 second GOSUB wait4it 'replaces PAUSE with loop that doesn't stop Stamp servodrive = SERVONOM GOSUB eyemove 'skip Eyes - if commented out GOSUB mouthmove 'skip Mouth - if commented out GOSUB updnmove 'skip UP_DN - if commented out GOSUB lrmove 'skip Left_Right - if commented out '*************************************** do_act: 'Cute little moving eyes and head trick ' GOTO loop5 'skip looking left if not commented out rampLR = 2 'Ramp rate to Left/Right servo rampUD = 8 'Ramp rate to Up/Down servo rampM = 8 'Ramp rate to Mouth servo rampE = 8 'Ramp rate to Eyes servo servodrive = SERVONOM WAIT_TIME = 50 '50*20ms = 1 second GOSUB wait4it 'replaces PAUSE with loop that doesn't stop Stamp '- Look left fast rampE = 25 'Ramp rate to Eyes servo GOSUB set_ramps 'setup servo ramps on channels channel = EYES 'Eyes Right = 0, Eyes Left = 255 servodrive = 255 GOSUB run_servo WAIT_TIME = 100 '100*20ms = 2 seconds GOSUB wait4it 'replaces PAUSE with loop that doesn't stop Stamp '- turn head and re-center eyes rampE = 3 'Ramp rate to Eyes servo GOSUB set_ramps 'setup servo ramps on channels channel = EYES 'Eyes Right = 0, Eyes Left = 255 servodrive = SERVONOM GOSUB run_servo channel = LEFT_RIGHT 'His Right = 20, His Left = 240 servodrive = 240 GOSUB run_servo WAIT_TIME = 250 '250*20ms = 5 seconds GOSUB wait4it 'replaces PAUSE with loop that doesn't stop Stamp '- reset head ' servodrive = SERVONOM ' GOSUB run_servo rampE = 25 'Ramp rate to Eyes servo GOSUB set_ramps 'setup servo ramps on channels channel = EYES 'Eyes Right = 0, Eyes Left = 255 servodrive = 0 GOSUB run_servo WAIT_TIME = 100 '100*20ms = 2 seconds GOSUB wait4it 'replaces PAUSE with loop that doesn't stop Stamp '- turn head back to center and re-center eyes rampE = 3 'Ramp rate to Eyes servo GOSUB set_ramps 'setup servo ramps on channels channel = EYES 'Eyes Right = 0, Eyes Left = 255 servodrive = SERVONOM GOSUB run_servo channel = LEFT_RIGHT 'His Right = 20, His Left = 240 servodrive = SERVONOM GOSUB run_servo WAIT_TIME = 250 '250*20ms = 5 seconds GOSUB wait4it 'replaces PAUSE with loop that doesn't stop Stamp loop5: ' GOTO loop6 'skip looking right if not commented out '- Look right fast rampE = 25 'Ramp rate to Eyes servo GOSUB set_ramps 'setup servo ramps on channels channel = EYES 'Eyes Right = 0, Eyes Left = 255 servodrive = 0 GOSUB run_servo WAIT_TIME = 100 '100*20ms = 2 seconds GOSUB wait4it 'replaces PAUSE with loop that doesn't stop Stamp '- turn head and re-center eyes rampE = 3 'Ramp rate to Eyes servo GOSUB set_ramps 'setup servo ramps on channels channel = EYES 'Eyes Right = 0, Eyes Left = 255 servodrive = SERVONOM GOSUB run_servo channel = LEFT_RIGHT 'His Right = 20, His Left = 240 servodrive = 20 GOSUB run_servo WAIT_TIME = 250 '250*20ms = 5 seconds GOSUB wait4it 'replaces PAUSE with loop that doesn't stop Stamp '- reset head '- Look left fast i.e. back to center rampE = 25 'Ramp rate to Eyes servo GOSUB set_ramps 'setup servo ramps on channels channel = EYES 'Eyes Right = 0, Eyes Left = 255 servodrive = 255 GOSUB run_servo ' GOSUB Debug_msg1 'show servo values for chan = "channel" WAIT_TIME = 100 '100*20ms = 2 seconds GOSUB wait4it 'replaces PAUSE with loop that doesn't stop Stamp '- turn head and re-center eyes rampE = 3 'Ramp rate to Eyes servo GOSUB set_ramps 'setup servo ramps on channels channel = EYES 'Eyes Right = 0, Eyes Left = 255 servodrive = SERVONOM GOSUB run_servo channel = LEFT_RIGHT 'His Right = 20, His Left = 240 servodrive = SERVONOM 'back to center GOSUB run_servo WAIT_TIME = 250 '250*20ms = 5 seconds GOSUB wait4it 'replaces PAUSE with loop that doesn't stop Stamp loop6: goto main '======================================================================== ' SUBROUTINES BELOW '======================================================================== Debug_msg1: 'display servo values SEROUT 10, 1021, [8 + channel] 'Read servo setpoint (where it wants to get) SERIN 9, 1021, [SerDIn] 'DEBUG "servo setpoint = ", DEC SerDIn SEROUT 10, 1021, [16 + channel] 'Read servo Ramp SERIN 9, 1021, [SerDIn] 'DEBUG ", servo ramp = ", DEC SerDIn SEROUT 10, 1021, [24 + channel] 'Read servo Position (Where it is now) SERIN 9, 1021, [SerDIn] 'DEBUG ", servo position = ", DEC SerDIn,cr, LF, LF WAIT_TIME = 100 '100*20ms = 2 seconds GOSUB wait4it 'replaces PAUSE with loop that doesn't stop Stamp RETURN '***** THIS ROUTINE REPLACES PAUSE - IT DOESN'T STOP WHILE WAITING ******* ' All I'm doing is checking for the Mode0 switch to be pressed or ' the timer to finish. Could do much more! wait4it: SEROUT 10, 1021, [128,WAIT_TIME] 'load Co-Proc Timer 0 in non-retrig mode ' for WAIT_TIME*20ms delay1: IF (robot_run = 1) THEN Do_other_stuff 'robot mode is already running INPUT 15 'look for mode0 switch IF IN15 = 1 THEN Do_other_stuff 'jump if switch not pressed freqout SPKR,500,2000,4000 'mode0 switch pressed - Beep the speaker GOSUB start_Libby 'start up the robot LOW 15 'Light LED on Port Mode0 as a reminder robot_run = 1 'remember the fact that the robot is running Do_other_stuff: 'Add your code if any other things to do during wait 'Left lots of time to do other things - check_timers: INPUT 11 'Timer_Complete input from Co-Processor IF IN11 = 0 THEN delay1 'jump back if timer not finished 'read timeout alarm byte to get and clear it SEROUT 10, 1021, [119] 'Send the byte command SERIN 9, 1021, [SerDIn] 'Data comes back into SerDIn Chk_for_Timer0: 'following needed if it could have been other timers too. 'we are only using timer 0 so it is not necessary ' IF (NOT(SerDIn & $01)) THEN Chk_Other_Timers 'Jump if not Timer 0. Chk_Other_Timers: 'Add your code to check if any other 'timers are in use too. RETURN 'pause is finished '*********** start_Libby: 'This really should be inline code since it is only 1 instruction. 'It is only kept separate here for clarity. ' To run Libby the robot at the same time as running Herbert the Head ' Write bot_control - this byte controls the Subsumption Architecture. ' The priorities from lowest to highest are shown below. ' The lowest level is randomly wandering around. ' Just above that is level 1, you write the direction and duration for level 1 ' into the co-Processor registers and it gets integrated into the Subsumption ' calculations. ' Then vision from the IR Proximity Detector. ' Level 3 is like Level 1 ' bumpers next, ' Finally the highest priority is level 5. ' The bits in the bot_control byte enable each of the levels when they are a 1. ' Ball Bearing determines the relative directions vs pulse width. Libby has ' ball bearing servos so it is set to 1. ' do_bot enables the subsumption engine. SEROUT 10, 1021, [117,218,$D5] 'this command is all that is needed to run the robot ' with IRPD, bumpers, and a subsumption engine 'command to initialize for robot mode (117), write bot_control & run the robot (218,$D5) ' D = Ball Bearing = 1; Show Vision = 1; level 5 = 0; do_bumpers = 1; ' 5 = level 3 = 0; do_vision = 1; level 1 = 0; do_bot = 1 RETURN 'go back to checking the timers for the head '*********** set_ramps: 'setup servo ramps ' DEBUG "Set the servos for ramping",CR,LF SEROUT 10, 1021, [(WRITE_IT+RAMP+2),rampLR] 'write rate to servo 2 = Left/Right ' (command = 128 + 16 + ch2 = 146) 'Ramp rate = x => incr/decr x/4 per 20 ms ' up to a max of x = 31 ' Fine 1->2ms mode selected SEROUT 10, 1021, [(WRITE_IT+RAMP+3),rampUD] 'write rate to servo 3 = 147 = Up/Down SEROUT 10, 1021, [(WRITE_IT+RAMP+4),rampM] 'write rate to servo 4 = 148 = Mouth SEROUT 10, 1021, [(WRITE_IT+RAMP+5),rampE] 'write rate to servo 5 = 149 = Eyes RETURN '*********** with_ramp: 'make servos run with ramping ' DEBUG "Running the servos with ramping",CR,LF ' DEBUG "Channel = ",DEC channel, CR,LF ' DEBUG "Start the servos toward SERVOMIN with ramping",CR,LF servodrive = SERVOMIN GOSUB run_servo GOSUB wait4it 'replaces PAUSE with loop that doesn't stop Stamp ' DEBUG "Start the servos toward SERVOMAX with ramping",CR,LF servodrive = SERVOMAX GOSUB run_servo GOSUB wait4it 'replaces PAUSE WAIT_TIME with loop that doesn't stop Stamp GOSUB wait4it 'replaces PAUSE WAIT_TIME with loop that doesn't stop Stamp servodrive = SERVONOM GOSUB run_servo freqout SPKR,250,500,4000 GOSUB wait4it 'replaces PAUSE WAIT_TIME with loop that doesn't stop Stamp RETURN '*********** run_servo SEROUT 10, 1021, [136+channel,servodrive] 'Send Servo command 'WRITE_IT + Servo SETPOINT + channel = ' 128 + 8 = 136 + channel RETURN '*********** eyemove: channel = EYES ' = 5 SERVOMIN = 30 ' = His Right SERVOMAX = 235 ' = His Left WAIT_TIME = 4000 GOSUB with_ramp 'make servos run with ramping ' DEBUG "Finished EYES",CR,LF RETURN '*************************************** mouthmove: channel = MOUTH ' = 4 SERVOMIN = 20 ' = MOUTH Open - these values depend on a specific head SERVOMAX = 240 ' = MOUTH Closed WAIT_TIME = 4000 GOSUB with_ramp 'make servos run with ramping ' DEBUG "Finished MOUTH",CR,LF RETURN '*************************************** updnmove: channel = UP_DN ' = 3 SERVOMIN = 10 ' = UP SERVOMAX = 240 ' = Down WAIT_TIME = 3000 GOSUB with_ramp 'make servos run with ramping ' DEBUG "Finished UP_DN",CR,LF RETURN '*************************************** lrmove: channel = LEFT_RIGHT ' = 2 SERVOMIN = 20 'His Right = 20, His Left = 240 SERVOMAX = 240 ' = His Left WAIT_TIME = 5000 GOSUB with_ramp 'make servos run with ramping ' DEBUG "Finished LEFT_RIGHT",CR,LF RETURN end