Script is crashing controller

Option Explicit

'Variable Declaration
dim Gain as integer			'Fine tuning of the steering sensitivity. Ensures we don't turn to quickly or slowly
dim DefaultThrottle as integer
dim TapeDetect as boolean	
dim MarkerLeft as boolean
dim MarkerRight as boolean	
dim Throttle as integer		'The current speed setting of the system
dim Tape_Position as integer
dim LineSelect as integer	
dim Steering as integer		'A value of steering to the right or left
dim LT_Speed as integer		'Left Motor speed
dim RT_Speed as integer		'Right motor Speed
dim AGVEngage as boolean	'The signal to tell the Roboteq controller to begin moving in AGV mode
dim NotOnStopMarker as boolean	
dim ObstructionDelay as integer	'This is how long to wait inbetween scans when there is an obstruction
dim OSSD1 as boolean		'SLS Output 1
dim OSSD2 as boolean		'SLS Output 2
dim MG_LT_DB as integer		'The left lower limit to begin steering
dim MG_RT_DB as integer		'The right lower limit to begin steering
dim SP_DB as integer		'The deadband for the motor speed commands. If motor speed is below this, we don't want them to move.
dim Manual_Drive as boolean	'Command from PLC to enter manual driving mode
'Configuration Parameters
Gain = 2	'Use a negative value to invert the steering
DefaultThrottle = 250	'The default forward throttle level
LineSelect = 2	'Use the right track by default
ObstructionDelay = 5000 'in milliseconds
MG_LT_DB = -5	'Limit on the misalignment of the magnetic tape to the left (Steering not affected by this much)
MG_RT_DB = 5	'Limit on misalignment to the right
SP_DB = 50

'Main Loop. repeat every 10ms
top:
wait(10)

'read sensor data
TapeDetect = getvalue(_MGD)
MarkerLeft = getvalue(_MGM, 1)
MarkerRight = getvalue(_MGM, 2)

'Tell the PLC if the tape has been detected
if TapeDetect <> 0 then
	SetCommand(_DSET, 3)
else
	SetCommand(_DRES, 3)
end if

Manual_Drive = getvalue(_DI, 8)
if Manual_Drive <> 0 then goto Man	'Jump to the manual command mode below

'Check to see if the PLC wants us to move
AGVEngage = getvalue(_DI, 5)
if AGVEngage = 0 then
	SetCommand(_DRES, 1)	'Turn off the "AGV ON" Signal to the PLC
	SetCommand(_G, 1, 0)		'Set left motor speed to 0
	SetCommand(_G, 2, 0)		'Set right motor speed to 0
	goto top 
else 
	SetCommand(_DSET,1)		'Send "AGV ON" signal to PLC to let it know we are moving
end if


'Get the Safety laser scanner status
OSSD1 = getvalue(_DI, 6)
OSSD2 = getvalue(_DI, 7)


'Check for faults
if OSSD1 = 0 then goto Obst
if OSSD2 = 0 then goto Obst


'Engage the throttle
if TapeDetect = True then
	Throttle = DefaultThrottle
else
	Throttle = 0
end if


'Check marker presence to select right or left track
if (MarkerLeft) then LineSelect = 1

if (MarkerRight) then LineSelect = 2


'Detect when transitioning onto stop markers
if (NotOnStopMarker and MarkerLeft and MarkerRight)

	NotOnStopMarker = false	' Mark the stop marker detection so it is not detected until the AGV has moved away
	
	SetCommand(_DSET, 4)	'Turn on output to PLC to let it know we have reached a stop marker
	
	Throttle = 0			'Cut the throttle to stop the AGV
	SetCommand(_GO, 1, 0)		'Set left motor speed to 0
	SetCommand(_GO, 2, 0)		'Set right motor speed to 0 
	
	goto top
else
	NotOnStopMarker = true
	SetCommand(_DRES, 4)	'Turn off the output to let the PLC know we are not on a stop marker
end if
	

'Get the tape position
Tape_Position = getvalue(_MGT, LineSelect)

Steering = Tape_Position * Gain

'Calculate the left and right speeds for the motor
if Tape_Position < MG_LT_DB then		'We need to make a left turn
	LT_Speed = Throttle - abs(Steering)		'Slow down the left motor
	RT_Speed = Throttle + abs(Steering)		'Speed up the right motor

elseif Tape_Position > MG_RT_DB then	'We need to turn to the right
	LT_Speed = Throttle + abs(Steering)		'Speed up the left motor
	RT_Speed = Throttle - abs(Steering)		'Slow down the right motor
else
	LT_Speed = Throttle
	RT_Speed = Throttle
end if

'Output the motor speeds to the motors unless the individual motor speed falls into the deadbands
if abs(LT_Speed) > SP_DB then
	setcommand(_G, 1, LT_Speed)		'Set left motor speed
end if

if abs(RT_Speed) > SP_DB then
	setcommand(_G, 2, RT_Speed)
end if

'return to the beginning of the main loop
goto top

'Manual mode signal handling
Man:

LT_Speed = getvalue(_AIC, 3)
RT_Speed = getvalue(_AIC, 4)

setcommand(_G, 1, LT_Speed)
setcommand(_G, 2, RT_Speed)

goto top

'Obstruction detected. Wait and jump back to top when delay has transpired
Obst:
setcommand(_GO, 1, 0)		'Set left motor speed to 0
setcommand(_GO, 2, 0)		'Set right motor speed to 0
setcommand(_DSET, 2)	'Send "AGV mode paused" signal to PLC
wait(ObstructionDelay)	'Wait for a bit before checking to see if the obstruction is clear.

'Check to see if the obstruction has cleared
OSSD1 = getvalue(_DI, 6)
OSSD2 = getvalue(_DI, 7)
if OSSD1 = 1 then goto Obst
if OSSD2 = 1 then goto Obst

'If we made it this far, the obstruction should be clear
setcommand(_DRES, 2)	'Turn off the "AGV mode paused" signal
goto top

Option Explicit
'Description: Runtime program for a line following robot

'Variable Declaration
dim Gain as integer			'Fine tuning of the steering sensitivity. Ensures we don't turn to quickly or slowly
dim DefaultThrottle as integer
dim TapeDetect as boolean	
dim MarkerLeft as boolean
dim MarkerRight as boolean	
dim Throttle as integer		'The current speed setting of the system
dim Tape_Position as integer
dim LineSelect as integer	
dim Steering as integer		'A value of steering to the right or left
dim LT_Speed as integer		'Left Motor speed
dim RT_Speed as integer		'Right motor Speed
dim AGVEngage as boolean	'The signal to tell the Roboteq controller to begin moving in AGV mode
dim NotOnStopMarker as boolean	
dim ObstructionDelay as integer	'This is how long to wait inbetween scans when there is an obstruction
dim OSSD1 as boolean		'SLS Output 1
dim OSSD2 as boolean		'SLS Output 2
dim MG_LT_DB as integer		'The left lower limit to begin steering
dim MG_RT_DB as integer		'The right lower limit to begin steering
dim SP_DB as integer		'The deadband for the motor speed commands. If motor speed is below this, we don't want them to move.
dim Manual_Drive as boolean	'Command from PLC to enter manual driving mode
dim OSSDCounter as integer         'Counter to determine if there is a drop in sls signal or the scanner has detected an object in the field
'Configuration Parameters
Gain = 2	'Use a negative value to invert the steering
DefaultThrottle = 250	'The default forward throttle level
LineSelect = 2	'Use the right track by default
ObstructionDelay = 3000 'in milliseconds
MG_LT_DB = -5	'Limit on the misalignment of the magnetic tape to the left (Steering not affected by this much)
MG_RT_DB = 5	'Limit on misalignment to the right
SP_DB = 50

'Main Loop. repeat every 10ms
top:
wait(10)

'read sensor data
TapeDetect = getvalue(_MGD)
MarkerLeft = getvalue(_MGM, 1)
MarkerRight = getvalue(_MGM, 2)

'Tell the PLC if the tape has been detected
if TapeDetect <> 0 then
	SetCommand(_DSET, 3)
else
	SetCommand(_DRES, 3)
end if

Manual_Drive = getvalue(_DI, 8)
if Manual_Drive <> 0 then goto Man	'Jump to the manual command mode below

'Check to see if the PLC wants us to move
AGVEngage = getvalue(_DI, 5)
if AGVEngage = 0 then
	SetCommand(_DRES, 1)	'Turn off the "AGV ON" Signal to the PLC
	SetCommand(_G, 1, 0)		'Set left motor speed to 0
	SetCommand(_G, 2, 0)		'Set right motor speed to 0
	goto top 
else 
	SetCommand(_DSET,1)		'Send "AGV ON" signal to PLC
end if


'Get the Safety laser scanner status
OSSD1 = getvalue(_DI, 6)
OSSD2 = getvalue(_DI, 7)


'Check for faults
if OSSD1 = 0 or OSSD2 = 0 then 
	goto Obst
else
	OSSDCounter = 0
end if


'Engage the throttle
if TapeDetect = True then
	Throttle = DefaultThrottle
else
	Throttle = 0
end if


'Check marker presence to select right or left track
if (MarkerLeft) then LineSelect = 1

if (MarkerRight) then LineSelect = 2


'Detect when transitioning onto stop markers
if (NotOnStopMarker and MarkerLeft and MarkerRight)

	NotOnStopMarker = false	' Mark the stop marker detection so it is not detected until the AGV has moved away
	
	SetCommand(_DSET, 4)	'Turn on output to PLC to let it know we have reached a stop marker
	
	Throttle = 0			'Cut the throttle to stop the AGV
	SetCommand(_G, 1, 0)		'Set left motor speed to 0
	SetCommand(_G, 2, 0)		'Set right motor speed to 0 
	
	goto top
else
	NotOnStopMarker = true
	SetCommand(_DRES, 4)	'Turn off the output to let the PLC know we are not on a stop marker
end if
	

'Get the tape position
Tape_Position = getvalue(_MGT, LineSelect)

Steering = Tape_Position * Gain

'Calculate the left and right speeds for the motor
if Tape_Position < MG_LT_DB then		'We need to make a left turn
	LT_Speed = Throttle - abs(Steering)		'Slow down the left motor
	RT_Speed = Throttle + abs(Steering)		'Speed up the right motor

elseif Tape_Position > MG_RT_DB then	'We need to turn to the right
	LT_Speed = Throttle + abs(Steering)		'Speed up the left motor
	RT_Speed = Throttle - abs(Steering)		'Slow down the right motor
else
	LT_Speed = Throttle
	RT_Speed = Throttle
end if

'Output the motor speeds to the motors unless the individual motor speed falls into the deadbands
if abs(LT_Speed) > SP_DB then
	'Handle any cases where the speed has been set out of bounds of the motor
	if LT_Speed > 1000 then
		LT_Speed = 1000
	elseif LT_Speed < -1000 then
		LT_Speed = -1000
	end if
	setcommand(_G, 1, LT_Speed)		'Set left motor speed
end if

if abs(RT_Speed) > SP_DB then
	if RT_Speed > 1000 then
		RT_Speed = 1000
	elseif RT_Speed < -1000 then
		RT_Speed = -1000
	end if
	setcommand(_G, 2, RT_Speed)
end if
'Output Debugging information
'print("\r Tape Detect: ", TapeDetect, "\r Tape Pos: ", Tape_Position, "\r Left Marker: ", MarkerLeft, "\r Right Marker: ", MarkerRight, "\r Throttle: ", Throttle, "\r Steering: ", Steering, "\r LineSelect: ", LineSelect, "\r NotOnStopMarker: ", NotOnStopMarker, "\r")
'print("\r LT Motor: ", LT_Speed, "\r RT Motor: ", RT_Speed, "\r")

'return to the beginning of the main loop
goto top

'Manual mode signal handling
Man:

LT_Speed = getvalue(_AIC, 3)
RT_Speed = getvalue(_AIC, 4)

setcommand(_G, 1, LT_Speed)
setcommand(_G, 2, RT_Speed)

goto top

'Obstruction detected. Wait and jump back to top when delay has transpired
Obst:
If OSSDCounter >= 10 then		'Check for OSSD channel flickering each count is approximately 10 ms
	setcommand(_G, 1, 0)		'Set left motor speed to 0
	setcommand(_G, 2, 0)		'Set right motor speed to 0
	setcommand(_DSET, 2)	'Send "AGV mode paused" signal to PLC
	wait(ObstructionDelay)	'Wait for a bit before checking to see if the obstruction is clear.
else
	OSSDCounter = OSSDCounter + 1
end if

'Check to see if the obstruction has cleared
OSSD1 = getvalue(_DI, 6)
OSSD2 = getvalue(_DI, 7)
if OSSD1 = 1 then goto Obst
if OSSD2 = 1 then goto Obst

'If we made it this far, the obstruction should be clear
setcommand(_DRES, 2)	'Turn off the "AGV mode paused" signal
goto top