Menu

Topic-icon Script is crashing controller

More
4 months 3 weeks ago #29534000 by tdollar

Hi,

I have an application where I am trying to develop an AGV with an HDC2460 and an MGSW1600. I have built in a manual mode bypass on the program to allow a user to send analog commands to the controller when manually driving the unit around. There is an onboard PLC sending signals between the digital inputs and outputs.

The program works great for manual control. However, when I engage AGV mode, the controller will occasionally crash depending on the location of the magnetic strip in relation to the sensor. Does anyone have any ideas why this may be happening? Thanks for the help!

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

Please Log in or Create an account to join the conversation.

More
4 months 3 weeks ago - 4 months 3 weeks ago #29534003 by Gabriel_Isko

I haven't looked through your script yet. Are you using tape detect to check whether the sensor sees the tape or not?


Yes I see that you are after glancing through your script. Can you describe the crashing behavior you mentioned? I am not sure what you mean. Do you mean that the script no longer operates, or that the AGV crashes into a wall?

Please Log in or Create an account to join the conversation.

More
4 months 3 weeks ago - 4 months 3 weeks ago #29534008 by tdollar

Turns out it had to do with the safety laser scanner (the microbasic script was working too well). I was getting a little flickering in the OSSD1 and OSSD2 signals from the laser scanner. The microbasic script was picking up the drop in signal and bringing the motors to a stop. I thought it was the roboteq controller crashing and rebooting. In fact, it was the delay due to an "Obstruction" that wasn't really there. The flicker was happening so fast, the laser scanner didn't actually register any issues.

To fix this issue, I ended up adding a counter to "ride" out the dither in signal from the OSSD inputs. The only drawback is that my safety system response time increases by the number of times I wait to confirm the breaking of the OSSD contact. I will have to adjust the stopping distance to accommodate this.

Additionally, I added some logic to prevent the SetCommand(_G, 1, LT_Speed) from ever going outside of the motor command bounds (-1000, 1000)

Here's the updated code.

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

Please Log in or Create an account to join the conversation.

More
1 week 6 days ago #29534446 by Gabriel_Isko

Glad you were able to fix this and find that the root cause was debounce.

Please Log in or Create an account to join the conversation.

Time to create page: 0.111 seconds
Go to top