Menu

Topic-icon Differential Steering

More
5 years 3 months ago #29529755 by jonohenebrey
Hi Guys,

Really having some trouble getting the steering right on this AGV I am trying to program and was hoping someone with some experience can give me some advice. I am building an AGV that uses two motors to steer, it uses the Roboteq MagSensor and MDC2230 Motor Controller to track the magnetic striping and steer accordingly, I haven't used encoders to give feed back from the motors. I am controlling the motors in separate mode and adjust the throttle speed according to the position the MagSensor feeds-back to the motor controller using the multiPWM connection. The AGV is really heavy due to two battery packs and what seems to be happening is that it follows the line perfectly up until the point at which it needs to turn, and due to the momentum the it fails to react quick enough, using a shallower turn helps somewhat but I would prefer that it could be more responsive to the corner. I am also having issues with the AGV picking up the markers, the scanner is about 25mm off the ground. Here is my code, I implemented a design where if the AGV diverged a certain distance from the centre point it would indicate it is turning and the parameters would change. Any advice would be greatly appreciated.
option explicit
' This script provide basic control for an AGV.
' Motor will turn on upon the presence of a track and stop when track disappears.
' The track position information is used to provide left/right steering.
' At forks, the AGV will follow the left or right track depending whether the last
' marker detected was on the left or right side of the track.
' Make sure you precede merges with a marker so that the AGV remains on the main track.
' The presence of a left and right marker simultaneously will cause the AGV to stop for
' 30 seconds or until the operator presses the button
' declare variables
dim Proportional as integer
dim Differential as integer
'dim Integral as integer
dim threshold as integer
dim turn_threshold as integer
dim PID as integer
dim DefaultThrottle as integer
dim DefaultThrottle_1 as integer
dim TapeDetect as boolean
dim Battery as integer
dim ThrottleOne as integer
dim ThrottleTwo as integer
dim Tape_Position as integer ' The error deviation from centre (0)
dim Prev_Tape_Position as integer
dim LineSelect as integer
dim GoButton as boolean
dim Slow as boolean
dim RunState as boolean
dim NotOnStopMarker as boolean
dim PauseTime as integer
dim MarkerCount as integer
dim MarkerCountR as integer
dim MarkerLeft as boolean
dim MarkerRight as boolean
dim PrevLeftMarker as integer
dim PrevRightMarker as integer
' initialize constants
' Use negative value to invert steering command
NotOnStopMarker = true
Proportional = 13
Differential = 3 

threshold = 2
turn_threshold = 18
'Integral = 1
Prev_Tape_Position = 0
DefaultThrottle = 180 ' Motor power level while the AGV runs
DefaultThrottle_1 = 210
LineSelect = 1 ' Use left track by default
PauseTime = 5000 ' in miliseconds

' main loop to repeat every 10ms
' main loop to repeat every 10ms
Slow = false
GoButton = true 'temporary until physical button implemented
RunState = true 
top:
'wait(10)
' read sensor data
TapeDetect = getvalue(_MGD)
MarkerLeft = getvalue(_MGM, 1)
MarkerRight = getvalue(_MGM, 2)
GoButton = getvalue(_DI, 2)
'Slow = getvalue(_DI, 5)
'----------------------------------------------------------------------
'If button Pressed Timer is cleared else waiting for 30 sec to elapse")
if (GoButton) then SetTimerCount(1, 0) ' Pressing the button will clear the pause timer
if GetTimerState(1) and GoButton then RunState = true 'is the timer still running? 1 means timer reached 0
'and/or stopped, 0 means timer is running
' When pause timer is cleared, AGV is allowed to run
' Use TapeDetect and Pause Timer to apply throttle or not
'If Tape present and timer cleared, set throttle to default right line
'Throttle must be on when tape_position is in centre
' Check Marker presence to select Left or Right 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 ' stop marker detection so that it is not detected again
	'until AGV moved away
	SetTimerCount(1, PauseTime) ' Load stop timer timout value
	RunState = false
	setcommand(_DRES, 1)
else
	NotOnStopMarker = true
	setcommand(_DSET, 1)
end if
'------------------------------ Speed Up -----------------------------------------
if(MarkerLeft = true) ' When left marker is present
	if (PrevLeftMarker = false)
		MarkerCount = 0 ' clear counter when left marker first appears
	elseif(MarkerRight = true and PrevRightMarker = false)
		MarkerCount++ ' inc counter at every appearance of right marker
	end if
elseif (PrevLeftMarker = true) ' Left marker is absent but was present previously
	if (MarkerCount = 1)
		' enter code for one count marker here
	elseif (MarkerCount = 2)
		' enter code for two count marker here
		DefaultThrottle = 150
	end if
end if

if(Slow = false)
	DefaultThrottle = 180
	DefaultThrottle_1 = 210
end if
'------------------------------ Speed Down -----------------------------------------
if(MarkerRight = true) ' When left marker is present
	if (PrevRightMarker = false)
		MarkerCountR = 0 ' clear counter when left marker first appears
	elseif(MarkerLeft = true and PrevLeftMarker = false)
		MarkerCountR++ ' inc counter at every appearance of right marker
	end if
elseif (PrevRightMarker = true) ' Left marker is absent but was present previously
	if (MarkerCountR = 1)
		' enter code for one count marker here
	elseif (MarkerCountR = 2)
		' enter code for two count marker here
		DefaultThrottle = DefaultThrottle-20
	end if
end if
if(Slow)
	DefaultThrottle = DefaultThrottle - 20
	DefaultThrottle_1 = DefaultThrottle_1 - 20
end if
' Save markers for change detection at next cycle
PrevLeftMarker = MarkerLeft
PrevRightMarker = MarkerRight
'-----------------------------------------------------------------------
Tape_Position = getvalue(_MGT, LineSelect)
PID = (Tape_Position*Proportional) +(Differential*((Tape_Position - Prev_Tape_Position)))'+(Integral*(Tape_Position/100))
Prev_Tape_Position = Tape_Position

if (TapeDetect and GoButton)'and RunState
	'setcommand(_DRES, 2)
	if (Tape_Position < threshold and Tape_Position > -threshold)
		ThrottleOne = DefaultThrottle 'Left
		ThrottleTwo = DefaultThrottle 'Right
	end if
'Create a threshold (reduce jerking)
	if(Tape_Position >= threshold  ) 'off centre to right (Steer left)
		ThrottleOne = DefaultThrottle -(PID*3) 'Left
		ThrottleTwo = DefaultThrottle_1 + (PID/2) 	'Right
	end if

	if(Tape_Position <= -threshold ) 'off centre to left (Steer right)
		ThrottleOne = DefaultThrottle_1 - (PID/2)	'Left
		ThrottleTwo = DefaultThrottle + (PID*3)	'Right
	end if
	if(ThrottleOne > 400)
		ThrottleOne = 400
	end if
	if(ThrottleTwo > 400)
		ThrottleTwo = 400
	end if
	if(Tape_Position > turn_threshold or Tape_Position < -turn_threshold)
		'ThrottleOne = (ThrottleOne * -1)'Invert Throttle
	'check if negative applied to both, swop cables and limit the wheels from going into reverse
		if(ThrottleOne < -180)
		ThrottleOne = -120
		end if
		if(ThrottleTwo < -180)
		ThrottleTwo = -120
		end if
	else 
		if(ThrottleOne < -180)
		ThrottleOne = 0
		end if
		if(ThrottleTwo < -180)
		ThrottleTwo = 0
		end if
	end if

else
	ThrottleOne = 0
	ThrottleTwo = 0
	'setcommand(_DSET, 2) 'Set this to flash at 1hz
end if
' Send throttle to eeach motor with controller Configured in Seperated mode
setcommand(_G, 1, ThrottleOne)
setcommand(_G, 2, ThrottleTwo)
'---------------------------------Low Battery---------------------------------------
Battery = getvalue(_VOLTS, 2) ' queries battery voltage under variable Battery

if Battery < 220 then
	setcommand(_DSET, 2) ' if battery voltage is less than 15V then set digital output 1 on
else
	setcommand(_DRES, 2) ' else reset digital output 1 to off.
	
end if

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

More
5 years 3 months ago #29529756 by TechSupport
Replied by TechSupport on topic Differential Steering
Have you tried changing to a different mixed mode. Remember that each mode will have different impacts on the steering based on the motor commands sent. In some cases, 1 mixed mode may work better with steering, whereas the other may work better for going straight.

If the sensors are having some issues picking up the markers, then you may want to adjust the sensors sensitivity to markers in the Magsensor utility.

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

More
5 years 1 month ago #29529902 by Tisevski
Replied by Tisevski on topic Differential Steering
Hi,
have you solved the problem?

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

More
5 years 1 month ago #29529903 by jonohenebrey
Replied by jonohenebrey on topic Differential Steering
Yes, I changed over to using Mixed Mode and had to play around with the PID parameters, I gained relative stability

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

More
5 years 1 month ago #29529904 by Tisevski
Replied by Tisevski on topic Differential Steering
would you send me your current script,please?
I 'm from Germany and currently working on a same project.

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

Moderators: tonysantoni
Time to create page: 0.093 seconds
Go to top