Wisker and temp sensor
southernstudent
Posts: 1
I am trying to put together a boi-bot that uses both wiskers and a temp sensor to put out a candle for penn states firefighting contest.
Here is what I am using: boi-bot rev D, wiskers and the MLX90614 Infrared Thermometer Module (90° FOV)
I am trying to have the wiskers navigate around and use the sensor to find the fire, and then the boe-bot will go and take out the fire.
here is the code I currently have, If anyone could help, that would be much appreciated
'
[ Title ]
' Robotics with the Boe-Bot - EscapingCorners.bs2
' Boe-Bot navigates out of corners by detecting alternating whisker presses.
' {$STAMP BS2} ' Stamp directive.
' {$PBASIC 2.5} ' PBASIC directive.
DEBUG "Program Running!"
'
[ Variables ]
Sensor CON 3
'
[ Constants ]
baud CON 84
xslave CON $35 'slave address
'
[ Variables ]
temperature VAR Word
tempL VAR temperature.LOWBYTE
tempH VAR temperature.HIGHBYTE
pec VAR Byte
pulseCount VAR Byte ' FOR...NEXT loop counter.
counter VAR Nib ' Counts alternate contacts.
old7 VAR Bit ' Stores previous IN7.
old5 VAR Bit ' Stores previous IN5.
'
[ Initialization ]
FREQOUT 4, 2000, 3000 ' Signal program start/reset.
counter = 1 ' Start alternate corner count.
old7 = 0 ' Make up old values.
old5 = 1
DO
' --- Detect Consecutive Alternate Corners
' See the "How EscapingCorners.bs2 Works" section that follows this program.
IF (IN7 <> IN5) THEN ' One or other is pressed.
IF (old7 <> IN7) AND (old5 <> IN5) THEN ' Different from previous.
counter = counter + 1 ' Alternate whisker count + 1.
old7 = IN7 ' Record this whisker press
old5 = IN5 ' for next comparison.
IF (counter > 4) THEN ' If alternate whisker count = 4,
counter = 1 ' reset whisker counter
GOSUB Back_Up ' and execute a U-turn.
GOSUB Turn_Left
GOSUB Turn_Left
ENDIF ' ENDIF counter > 4.
ELSE ' ELSE (old7=IN7) or (old5=IN5),
counter = 1 ' not alternate, reset counter.
ENDIF ' ENDIF (old7<>IN7) and
' (old5<>IN5).
ENDIF ' ENDIF (IN7<>IN5).
' --- Same navigation routine from RoamingWithWhiskers.bs2
IF (IN5 = 0) AND (IN7 = 0) THEN ' Both whiskers detect obstacle
GOSUB Back_Up ' Back up & U-turn (left twice)
GOSUB Turn_Left
GOSUB Turn_Left
ELSEIF (IN5 = 0) THEN ' Left whisker contacts
GOSUB Back_Up ' Back up & turn right
GOSUB Turn_Right
ELSEIF (IN7 = 0) THEN ' Right whisker contacts
GOSUB Back_Up ' Back up & turn left
GOSUB Turn_Left
ELSE ' Both whiskers 1, no contacts
GOSUB Forward_Pulse ' Apply a forward pulse
ENDIF ' and check again
LOOP
'
[ Subroutines ]
Forward_Pulse: ' Send a single forward pulse.
PULSOUT 13,850
PULSOUT 12,650
PAUSE 20
RETURN
Turn_Left: ' Left turn, about 90-degrees.
FOR pulseCount = 0 TO 20
PULSOUT 13, 650
PULSOUT 12, 650
PAUSE 20
NEXT
RETURN
Turn_Right:
FOR pulseCount = 0 TO 20 ' Right turn, about 90-degrees.
PULSOUT 13, 850
PULSOUT 12, 850
PAUSE 20
NEXT
RETURN
Back_Up: ' Back up.
FOR pulseCount = 0 TO 40
PULSOUT 13, 650
PULSOUT 12, 850
PAUSE 20
NEXT
RETURN
'
[ Subroutines ]
getTemperature:
SEROUT Sensor,baud,[0,"!TEMR",xslave,$07]
SERIN Sensor,baud,1000,PECfail,[tempL,tempH,pec]
IF (temperature > 21000) THEN
'Forward_Pulse: ' Send a single forward pulse.
PULSOUT 13,850
PULSOUT 12,650
PAUSE 20
RETURN
'Back_Up: ' Back up.
FOR pulseCount = 0 TO 40
PULSOUT 13, 650
PULSOUT 12, 850
PAUSE 20
ENDIF
LOOP
Here is what I am using: boi-bot rev D, wiskers and the MLX90614 Infrared Thermometer Module (90° FOV)
I am trying to have the wiskers navigate around and use the sensor to find the fire, and then the boe-bot will go and take out the fire.
here is the code I currently have, If anyone could help, that would be much appreciated
'
[ Title ]
' Robotics with the Boe-Bot - EscapingCorners.bs2
' Boe-Bot navigates out of corners by detecting alternating whisker presses.
' {$STAMP BS2} ' Stamp directive.
' {$PBASIC 2.5} ' PBASIC directive.
DEBUG "Program Running!"
'
[ Variables ]
Sensor CON 3
'
[ Constants ]
baud CON 84
xslave CON $35 'slave address
'
[ Variables ]
temperature VAR Word
tempL VAR temperature.LOWBYTE
tempH VAR temperature.HIGHBYTE
pec VAR Byte
pulseCount VAR Byte ' FOR...NEXT loop counter.
counter VAR Nib ' Counts alternate contacts.
old7 VAR Bit ' Stores previous IN7.
old5 VAR Bit ' Stores previous IN5.
'
[ Initialization ]
FREQOUT 4, 2000, 3000 ' Signal program start/reset.
counter = 1 ' Start alternate corner count.
old7 = 0 ' Make up old values.
old5 = 1
DO
' --- Detect Consecutive Alternate Corners
' See the "How EscapingCorners.bs2 Works" section that follows this program.
IF (IN7 <> IN5) THEN ' One or other is pressed.
IF (old7 <> IN7) AND (old5 <> IN5) THEN ' Different from previous.
counter = counter + 1 ' Alternate whisker count + 1.
old7 = IN7 ' Record this whisker press
old5 = IN5 ' for next comparison.
IF (counter > 4) THEN ' If alternate whisker count = 4,
counter = 1 ' reset whisker counter
GOSUB Back_Up ' and execute a U-turn.
GOSUB Turn_Left
GOSUB Turn_Left
ENDIF ' ENDIF counter > 4.
ELSE ' ELSE (old7=IN7) or (old5=IN5),
counter = 1 ' not alternate, reset counter.
ENDIF ' ENDIF (old7<>IN7) and
' (old5<>IN5).
ENDIF ' ENDIF (IN7<>IN5).
' --- Same navigation routine from RoamingWithWhiskers.bs2
IF (IN5 = 0) AND (IN7 = 0) THEN ' Both whiskers detect obstacle
GOSUB Back_Up ' Back up & U-turn (left twice)
GOSUB Turn_Left
GOSUB Turn_Left
ELSEIF (IN5 = 0) THEN ' Left whisker contacts
GOSUB Back_Up ' Back up & turn right
GOSUB Turn_Right
ELSEIF (IN7 = 0) THEN ' Right whisker contacts
GOSUB Back_Up ' Back up & turn left
GOSUB Turn_Left
ELSE ' Both whiskers 1, no contacts
GOSUB Forward_Pulse ' Apply a forward pulse
ENDIF ' and check again
LOOP
'
[ Subroutines ]
Forward_Pulse: ' Send a single forward pulse.
PULSOUT 13,850
PULSOUT 12,650
PAUSE 20
RETURN
Turn_Left: ' Left turn, about 90-degrees.
FOR pulseCount = 0 TO 20
PULSOUT 13, 650
PULSOUT 12, 650
PAUSE 20
NEXT
RETURN
Turn_Right:
FOR pulseCount = 0 TO 20 ' Right turn, about 90-degrees.
PULSOUT 13, 850
PULSOUT 12, 850
PAUSE 20
NEXT
RETURN
Back_Up: ' Back up.
FOR pulseCount = 0 TO 40
PULSOUT 13, 650
PULSOUT 12, 850
PAUSE 20
NEXT
RETURN
'
[ Subroutines ]
getTemperature:
SEROUT Sensor,baud,[0,"!TEMR",xslave,$07]
SERIN Sensor,baud,1000,PECfail,[tempL,tempH,pec]
IF (temperature > 21000) THEN
'Forward_Pulse: ' Send a single forward pulse.
PULSOUT 13,850
PULSOUT 12,650
PAUSE 20
RETURN
'Back_Up: ' Back up.
FOR pulseCount = 0 TO 40
PULSOUT 13, 650
PULSOUT 12, 850
PAUSE 20
ENDIF
LOOP