' -----[ Title ]-------------------------------------------------------------- ' File: BoeBotControlForMsrs.bs2 ' {$STAMP BS2} ' {$PBASIC 2.5} ' -----[ DATA ]--------------------------------------------------------------- ResetOnOff DATA 0 ' On/off toggle w/ Reset button RequestConnect DATA $FF, 0, 1, 0, 0 ConnectionGranted DATA $FF, 0, 2, 0, 0 RequestCommand DATA $FF, 0, 3, 0, 0 ServoSpeeds DATA $FF, 0, 4, 0, 0 ' -----[ Variables ]---------------------------------------------------------- tLeft VAR Word ' Servo control pulse durations tRight VAR Word temp VAR Word ' Temp variable ' Buffer array not declared as buffer VAR Word(5) for SERIN functionality. ' It can still be accessed as buffer(0), buffer(1), etc. However, ' buffer0, buffer1, etc. should be used in SERIN commands with variations ' of WAIT. buffer0 VAR Byte ' Buffer - Start char = $ff buffer1 VAR Byte ' Message Index value buffer2 VAR Byte ' Command buffer3 VAR Byte ' Argument 1 (return data 1) buffer4 VAR Byte ' Argument 2 (return data 2) buffer VAR buffer0 ' For standard array indexing duration VAR Byte ' 50ths of ms duration frequency VAR Byte ' 50ths of frequency pointer VAR Byte ' EEPROM pointer msgIndex VAR Byte ' EEPROM message index routine VAR Nib ' Routine selector counter VAR Nib index VAR Nib flagWhiskers VAR Bit ' Act on Whisker values flagDigSens VAR Bit ' Send back digital sensors 1/0s ' with every reply flagIr VAR Bit ' tempBit VAR Bit ' -----[ Initialization ]----------------------------------------------------- ' This code makes it possible to toggle the Boe-Bot on/off by pressing and ' releasing the Board of Education's Reset button. Program_Start: ' For on/off toggle with Reset ' READ ResetOnOff, temp ' temp = temp + 1 ' WRITE ResetOnOff, temp ' IF temp.BIT0 = 0 THEN ' FREQOUT 4, 1000, 2000 - Indicate inactive ' END ' ENDIF HIGH 15 FREQOUT 4, 75, 3000 LOW 15 Reset: DEBUG CLS 'Wait for the eb500 radio to be ready. PAUSE 1000 'Wait for the EB500 Bluetooth connection to be established. DO UNTIL IN5 = 1: LOOP GOSUB Ram_Clear Request_Connect: pointer = RequestConnect GOSUB Get_Packet SEROUT 1, 84, [STR buffer \5] Confirm_Connect: pointer = ConnectionGranted GOSUB Get_Packet Wait_For_Confirm: SEROUT 1, 84, [255, 0, 1, 0, 0] SERIN 0, 84, 20, Wait_For_Confirm, [WAITSTR buffer \ 2, buffer2, buffer3, buffer4] Request_Packet: msgIndex = msgIndex + 1 pointer = RequestCommand GOSUB Get_Packet SEROUT 1, 84, [STR buffer \5] ' -----[ Main Routine ]------------------------------------------------------- DO Resume: ' If Message not rcvd, try again IF IN5 = 0 THEN Reset ' EB500 disconnected? PULSOUT 13, tLeft ' Servo control pulses PULSOUT 12, tRight SERIN 0, 84, 5, Resume, ' Get next command [WAITSTR buffer \ 2, buffer2, buffer3, buffer4] PULSOUT 13, tLeft ' Servo control pulses again PULSOUT 12, tRight IF buffer2 = 192 THEN ' Handle restart req from PC msgIndex = 0 GOTO Request_Connect ENDIF LOOKDOWN buffer2,[32, 33, 64, 65, 66, 96, 97, 98, 128, 129, 130, 160, 161, 162], routine ON routine GOSUB Set_Servo_Speed, Maneuver, Get_Ir, Get_Whiskers, Get_Pin_States, Speaker_Tone, Set_Pins, Delay, Enable_Digital_Sensors, Enable_Ir, Enable_Whiskers, Disable_Digital_Sensors, Disable_Ir, Disable_Whiskers ' Load digital sensor values into buffer byte 3 for return message. IF flagDigSens = 1 THEN GOSUB Digital_Sensors ' Increment message index for reply. Next message from PC has to use ' reply's buf[1]. msgIndex = msgIndex + 1 buffer1 = msgIndex SEROUT 1, 84, [STR buffer \5] LOOP ' -----[ Subroutine Digital_Sensors ]----------------------------------------- ' This subroutine is used by the main routine to return digital sensor values ' with each reply. Digital_Sensors: buffer3 = 0 GOSUB Get_Ir GOSUB Get_Whiskers IF ((buffer3.BIT2 = 0 OR buffer3.BIT3 = 0) AND (flagWhiskers = 1)) THEN tLeft = tLeft MAX 750 tRight = tRight MIN 750 ENDIF RETURN ' -----[ Subroutine Set_Servo_Speed ]----------------------------------------- ' Range of 0 to 200 with 100 = stopped maps to 650 to 850 with 750 stopped. Set_Servo_Speed: tLeft = buffer3 + 650 ' Decode servo speed. tRight = buffer4 + 650 RETURN ' -----[ Subroutine Speaker_Tone ]-------------------------------------------- Speaker_Tone: duration = buffer3 ' Decode speaker tone frequency = buffer4 FREQOUT 4, duration * 50, frequency * 50 ' Transmit tone RETURN ' Go to resume routine ' -----[ Subroutine Get_Ir ]-------------------------------------------------- ' IR object detection for buf[3] bits 1 (left) and 0 (right). Get_Ir: ' IF flagIr THEN FREQOUT 9, 1, 38500 ' IR headlight tempBit = IN10 ' IR receiver buffer3.BIT1 = tempBit ' Left IR reply bit FREQOUT 3, 1, 38500 tempBit = IN2 buffer3.BIT0 = tempBit ' ENDIF RETURN ' -----[ Subroutine Get_Packet ]---------------------------------------------- ' Loads packet into buffer for transmitting. Used by initialization. Get_Packet: FOR index = 0 TO 4 READ pointer + index, buffer(index) NEXT buffer(1) = msgIndex RETURN ' -----[ Subroutine Display_Command ]----------------------------------------- ' For debugging. Display_Command: ' FOR index = 0 TO 4 ' DEBUG DEC buffer(index), CR ' NEXT RETURN ' -----[ Subroutine - Check_Whiskers ]---------------------------------------- ' Stores left and right wiskers (contact switches) for buf[3] bit 3 (left) and ' 2 (right). Get_Whiskers: 'IF FlagWhiskers THEN buffer3.BIT3 = IN7 ' Left whisker buffer3.BIT2 = IN8 ' Right whisker 'ENDIF RETURN ' -----[ Subroutine - Get_Pin_States ]------------------------------------- ' Returns all I/O pin states. MSRS has to select the bit it wants. Get_Pin_States: buffer3 = INL ' P0..P7 buffer4 = INH ' P8..P16 RETURN ' -----[ Subroutine - Set_Pins ]---------------------------------------------- ' Sets up to two pins. buf[3] high nibble specifies the operation, and the ' low nibble specifies the pin. The same applies to buf[4]. ' High nibble: ' 0 - no action ' 1 - set to output ' 2 - set to input ' 3 - set to (output-high) ' 4 - set to (output-low) ' Low Nibble: ' 0 to 15 - Specifies I/O pin. ' DO NOT try to set 0, 1, 5, or 6. Set_Pins: temp.NIB1 = buffer3.HIGHNIB temp.NIB0 = buffer3.LOWNIB GOSUB Op_Pins temp.NIB1 = buffer4.HIGHNIB temp.NIB0 = buffer4.LOWNIB GOSUB Op_Pins RETURN Op_pins: SELECT temp.NIB1 CASE 1 OUTPUT temp.NIB0 CASE 2 INPUT temp.NIB0 CASE 3 HIGH temp.NIB0 CASE 4 LOW temp.NIB0 ENDSELECT RETURN ' -----[ Subroutine - Delay ]------------------------------------------------- ' Specify delay in ms. Delay: temp.LOWBYTE = buffer3 temp.HIGHBYTE = buffer4 PAUSE temp RETURN ' -----[ Subroutine - Enable_Digital_Sensors ]-------------------------------- ' Causes program to put digital sensor values in buf[3] of the reply packet. Enable_Digital_Sensors: FlagDigSens = 1 RETURN ' -----[ Subroutine - Disable_Digital_Sensors ]------------------------------ ' Causes program to put digital sensor values in buf[3] of the reply packet. Disable_Digital_Sensors: FlagDigSens = 0 RETURN ' -----[ Subroutine Enable_Whiskers ]----------------------------------------- ' Causes program to stop the Boe-Bot if Whisker contact is sensed. This gives ' the PC bluetooth system time to reply. Enable_Whiskers: flagWhiskers = 1 RETURN ' -----[ Subroutine Disable_Whiskers ]---------------------------------------- ' Causes program to ignore the whisker contact. Also ignores when they are ' not connected. Disable_Whiskers: flagWhiskers = 0 RETURN ' -----[ Subroutine Enable_Ir ]----------------------------------------------- ' Not implemented. Enable_Ir: flagIr = 1 RETURN ' -----[ Subroutine Enable_Whiskers ]----------------------------------------- ' Not implemented. Disable_Ir: flagIr = 0 RETURN ' -----[ Subroutine - Maneuver ]---------------------------------------------- ' Preprogrammed maneuver example. Current setup allows: ' "U" - Back up then U-turn ' "R" - Back up then right turn ' "L" - Back up then left turn Maneuver: FOR temp = 0 TO 35 PULSOUT 13, 650 PULSOUT 12, 850 PAUSE 20 NEXT SELECT buffer3 CASE "U" FOR temp = 0 TO 40 PULSOUT 13, 650 PULSOUT 12, 650 PAUSE 20 NEXT CASE "R" FOR temp = 0 TO 20 PULSOUT 13, 850 PULSOUT 12, 850 PAUSE 20 NEXT CASE "L" FOR temp = 0 TO 20 PULSOUT 13, 650 PULSOUT 12, 650 PAUSE 20 NEXT ENDSELECT RETURN ' -----[ Subroutine Ram_Clear ]----------------------------------------------- ' Clears ram for reset from PC. This prevents retaining the msgIndex and ' pulseLeft/right variable values, which would otherwise cause problems. Ram_Clear: W0 = 0 W1 = 0 W2 = 0 W3 = 0 W4 = 0 W5 = 0 W6 = 0 W7 = 0 W8 = 0 W9 = 0 W10 = 0 W11 = 0 W12 = 0 RETURN