Файл: Advanced Robotics with the Toddler (Paralax, student guide, v1.3, 2004).pdf

Скачать файл (7,75Мб)

Заказать решение

ВУЗ: Не указан

Категория: Не указан

Дисциплина: Не указана

Добавлен: 12.06.2025

Просмотров: 1928

Скачиваний: 0

ВНИМАНИЕ! Если данный файл нарушает Ваши авторские права, то обязательно сообщите нам.

		Chapter #8: Real-Time Programming with Bumpers and Infrared · Page 179

'	-----[ Main Code ]-------------------------------------------------------
Main_Program:
	DO
	GOSUB	Do_Vision	' look for obstacles with IR
	GOSUB	Do_Bumper	' check for obstacles with bumpers
	GOSUB	New_Movemt	' generates next move from table
	GOSUB	Do_Movement	' gives servo pulses out
	PAUSE	MoveDelay	' makes up 20 ms servo timing loop
	LOOP
END
'	-----[ Subroutines ]-----------------------------------------------------
Do_Vision:
	FREQOUT	LeftIRLED,1,38500	' pulse left IRLED.
	sensors.BIT0 = LeftIRSensor		' store IR detector output in RAM
	FREQOUT	RightIRLED,1,38500	' repeat for the right IR pair.
	sensors.BIT1 = RightIRSensor

' Check if currently doing an infrared move

IF ((leftIR_Flag = 1) & (rightIR_Flag = 1)) THEN See_Both

IF (leftIR_Flag = 1) THEN See_Left ' is left only?

IF (rightIR_Flag = 1) THEN See_Right ' is right only?

'Load IR detector output values into the lower 2 bits of the sensors

'variable, storing a number between 0 and 3 that the BRANCH command

'can execute the appropriate routine.

BRANCH sensors,[See_Both,See_Right,See_Left,See_None]

See_Both:
newMove = PivR
rightIR_Flag = 1		' flag: 1	= something on right side
leftIR_Flag = 1		' flag: 1	= something left side
HIGH	LeftVisionLED	' left vision sequence display LED
HIGH	RightVisionLED	' right vision sequence display LED
RETURN
See_Right:
newMove = PivL
rightIR_Flag = 1		' flag: 1	= something on right side
HIGH	RightVisionLED	' right vision sequence display LED
RETURN
See_Left:
newMove = PivR
leftIR_Flag = 1		' flag: 1	= see something left side

Page 180 · Advanced Robotics with the Toddler

HIGH	LeftVisionLED	' left vision sequence display LED
RETURN
See_None:
newMove = Fwd
RETURN
Do_Bumper:
DIRS =	prepBumper
OUTS =	bothLEDsOff

'Bumpers on each foot. A hit causes Toddler to back and turn.

'Code asssumes leg that hit is on the leg that is moving

'(while leaning on the other leg).

'A hit backs up the hit leg right away then steps back 1 step

'and does a pivot away.

IF (leftBumpFlag = 1) THEN Bump_LeftA

IF (rightBumpFlag = 1) THEN Bump_RightA

'check for old bumper hit on left

'check for old bumper hit on right

IF (LeftBumper = 0) THEN Bump_Left		' check for bumper hit on left
IF (RightBumper = 0) THEN Bump_Right		' check for bumper hit on right
DIRS =	ctrlLEDs
OUTS =	bothGreen
RETURN
Bump_Left:
'DEBUG	"bump left", cr	' sends only once on change
Bump_LeftA:
leftBumpFlag = 1		' flag indicates bump response move
newMove = BumpL
DIRS =	ctrlLEDs
OUTS =	leftRed
GOTO Clr_Vision		' goes to Clr_Vision
Bump_Right:
'DEBUG	"bump right", cr
Bump_RightA:
rightBumpFlag = 1
newMove = BumpR
DIRS =	ctrlLEDs
OUTS =	rightRed
		' falls through to Clr_Vision
'---------------

	Chapter #8: Real-Time Programming with Bumpers and Infrared · Page 181

Clr_Vision:		' reset	vision LEDs and flags
rightIR_Flag = 0		' flag:	1 = something on right side
leftIR_Flag = 0		' flag:	1 = something left side
LOW	LeftVisionLED	' left vision sequence display LED
LOW	RightVisionLED	' right	vision sequence display LED
RETURN
'---------------
New_Movemt:		' sequences for forward motion
IF (newMove <> currentMove) THEN		' new DATA if movement changed
'DEBUG cr, "start NewMovemt; newMove = ", DEC newMove
'DEBUG ", currentMove = ", DEC currentMove,			CR
doneMoveFlag = 1		' stop current move wherever it is
currentMove = newMove

'When customizing the program, here is where to integrate between

'moves. You can make the transitions reasonable by using the existing

'Mx value and MxOffset to compute the proper MxOffset to start into

'the next move.

IF ((newMove = Fwd) OR (newMove = BumpL) OR (newMove = BumpR)) THEN MxOffset = 0 ' reset MxOffset value for new move

ENDIF

' note: kept old MxOffset value for lead into pivot left or right

LOOKUP newMove,[Forward,PivotLeft,PivotRight,LBumper,RBumper],Mx

					'	lookup movement table index
ENDIF
IF (doneMoveFlag			= 1) THEN		'	done moving - lookup new move
READ (Mx	+ MxOffset), bMovmnt				'	read next basic move byte
'DEBUG "Mx = ", HEX Mx, ", MxOffset =						", HEX MxOffset
MxOffset	= MxOffset + 1
IF (bMovmnt		< xx) THEN			'	end code so do			new move
'DEBUG	"	bMovmnt = ",		HEX bMovmnt,		"	"
SELECT	bMovmnt				'	set and		display movement type
CASE	TL	: newTilt =		LeftTilt			: 'DEBUG		"TL, ", cr
CASE	TC	: newTilt =		CenterTilt			: 'DEBUG		"TC, ", cr
CASE	TR	: newTilt =		RightTilt			: 'DEBUG		"TR, ", cr
CASE	SL	: newStride		= LeftStride			: 'DEBUG		"SL, ", cr
CASE	SC	: newStride		= CenterStride			: 'DEBUG		"SC, ", cr
CASE	SR	: newStride		= RightStride			: 'DEBUG		"SR, ", cr
ENDSELECT
					'	fall through if invalid index

Page 182 · Advanced Robotics with the Toddler

ELSE
'DEBUG " xx = finished table", CR,		CR
MxOffset = 0	'	finished move sequence so restart
leftBumpFlag = 0	'	clear out left bumper hit flag
rightBumpFlag = 0	'	right too
DIRS = ctrlLEDs
OUTS = bothGreen
GOSUB Clr_Vision	'	reset vision LEDs and flags
GOSUB Do_Vision	'	since done previous movement,
GOSUB Do_Bumper	'	figure out new one
GOTO New_Movemt	'	get new table and offset
ENDIF
ENDIF
'Done_New_Move:
RETURN	'	ignore if invalid

'---------------
Do_Movement:
doneMoveFlag = 0	'	reset flags
doneTiltFlag = 0
IF (currentTilt < newTilt) THEN	'	start tilt calculations
currentTilt = currentTilt + TiltStep'		inc if coming from right tilt
IF (currentTilt >= newTilt) THEN	'	done?
currentTilt = newTilt	'	stop at limit
doneTiltFlag = 1
ENDIF
ELSE
currentTilt = currentTilt - TiltStep'		coming from left tilt = decrement
IF (currentTilt <= newTilt) THEN	'	done?
currentTilt = newTilt	'	stop at limit
doneTiltFlag = 1