FollowingBoeBot

FollowingBoeBot - ' ­­­­­[ Title

Info iconThis preview shows page 1. Sign up to view the full content.

View Full Document Right Arrow Icon
This is the end of the preview. Sign up to access the rest of the document.

Unformatted text preview: ' ­­­­­[ Title ]­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­ ' Robotics with the Boe­Bot ­ FollowingBoeBot.bs2 ' Boe­Bot adjusts its position to keep objects it detects in zone 2. ' {$STAMP BS2} ' Stamp directive. ' {$PBASIC 2.5} ' PBASIC directive. ' ­­­­­[ Constants ]­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­ Kpl CON ­35 Kpr CON 35 SetPoint CON 2 CenterPulse CON 750 ' ­­­­­[ Variables ]­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­ freqSelect VAR Nib irFrequency VAR Word irDetectLeft VAR Bit irDetectRight VAR Bit distanceLeft VAR Nib distanceRight VAR Nib pulseLeft VAR Word pulseRight VAR Word ' ­­­­­[ Initialization ]­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­ FREQOUT 4, 2000, 3000 ' ­­­­­[ Main Routine ]­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­ DO GOSUB Get_Ir_Distances ' Calculate proportional output. pulseLeft = SetPoint ­ distanceLeft * Kpl + CenterPulse pulseRight = SetPoint ­ distanceRight * Kpr + CenterPulse GOSUB Send_Pulse LOOP ' ­­­­­[ Subroutine ­ Get IR Distances ]­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­ Get_Ir_Distances: distanceLeft = 0 distanceRight = 0 FOR freqSelect = 0 TO 4 LOOKUP freqSelect,[37500,38250,39500,40500,41500], irFrequency FREQOUT 8,1,irFrequency irDetectLeft = IN9 distanceLeft = distanceLeft + irDetectLeft FREQOUT 2,1,irFrequency irDetectRight = IN0 distanceRight = distanceRight + irDetectRight NEXT RETURN ' ­­­­­[ Subroutine Get Pulse ]­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­ Send_Pulse: PULSOUT 13,pulseLeft PULSOUT 12,pulseRight PAUSE 5 RETURN ...
View Full Document

Ask a homework question - tutors are online