Differential Steering

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