' -----[ 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 100
FREQOUT 4, 75, 3000
'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 16, 84, [STR buffer \5]
Confirm_Connect:
pointer = ConnectionGranted
GOSUB Get_Packet
Wait_For_Confirm:
SEROUT 16, 84, [255, 0, 1, 0, 0]
SERIN 16, 84, 20, Wait_For_Confirm, [WAITSTR buffer \ 2,
buffer2, buffer3,
buffer4]
Request_Packet:
msgIndex = msgIndex + 1
pointer = RequestCommand
GOSUB Get_Packet
SEROUT 16, 84, [STR buffer \5]
' -----[ Main Routine ]-------------------------------------------------------
DO
Resume: ' If Message not rcvd, try again
'IF IN5 = 0 THEN Reset ' EB500 disconnected?
DEBUG " resume ",CR
PULSOUT 12, tLeft ' Servo control pulses
PULSOUT 13, tRight
FREQOUT 4, 500, 3000
SERIN 16, 84, 5, Resume, ' Get next command
[WAITSTR buffer \ 2, buffer2,
buffer3, buffer4]
PULSOUT 12, tLeft ' Servo control pulses again
PULSOUT 13, 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
DEBUG " do something ",CR
FREQOUT 4, 75, 3000,2000
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 16, 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:
DEBUG " f_ing move now ",CR
FREQOUT 4, 500,2000, 3000
tLeft = buffer4 + 650 ' Decode servo speed.
tRight = buffer3 + 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 0 (left) and 1 (right).
Get_Ir:
' IF flagIr THEN
FREQOUT 6, 1, 38500 ' IR headlight left
tempBit = IN0 ' IR receiver left
buffer3.BIT0 = tempBit ' Left IR reply bit
FREQOUT 7, 1, 38500 'right IR out
tempBit = IN3 'right IR in
buffer3.BIT1 = tempBit 'right bit
' 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:
' cut when done\/
FOR index = 0 TO 4
DEBUG DEC buffer(index), CR
NEXT
' cut when done/\
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.Bit2 = IN1 ' Left whisker
' buffer3.Bit3 = IN2 ' 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. bluetooth only!
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:
' fixed
SELECT buffer3
CASE "U"
FOR temp = 0 TO 35
PULSOUT 13, 650
PULSOUT 12, 850
PAUSE 20
NEXT
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
|
|
This Thread
