'*************************************************************
'ODOMETERS - Called from main loop or other subs - Too many calcs for multitask?
'However, somehow we need to get stable timed readings from encoders to get CurrentSpeeds??
'*************************************************************
Sub Odometers()
	'Read current tic values, calculate current X/Y/Theta, accumulate total CM traveled
	'We also calc the target distance and heading error after getting new location
	Dim XDistance		As Single		'Temp X for calcs in Odometer sub
	Dim YDistance		As Single		'Temp Y for calcs in Odometer sub
	Dim LeftSample 	as Long			'Current left tic reading
	Dim RightSample 	as Long			'Current right tic reading
	Dim CurLeftTics	As Long			'Local variable to get the current tics
	Dim CurRightTics	As Long

	'*********************************************
	'Take care of odometer updates here
	'Could not get to work as multi-tasking sub??
	'Also RAM limitations in multi-task
	'Read current tic count
	LeftSample	= LeftTicCount
	RightSample	= RightTicCount

	'Get how many tics since last reading
	CurLeftTics 	= LeftSample 		- LastLeftTic
	CurRightTics	= RightSample 	- LastRightTic

	'Update/save last read for next time
	LastLeftTic 	= LeftSample
	LastRightTic	= RightSample

	'Convert tics to measureable units
	'Initially I planned on using Centimeters (CM) but my mind still can think in metric so we are actually measuring inches now
	LeftCM	=	CSng(CurLeftTics)  / LeftTicsPerCM		'Note that we store Tics in long but calc CM in single so convert
	RightCM	=	CSng(CurRIghtTics) / RightTicsPerCM

	'Calc distance we have traveled since last sample
	'This is the left + right distance divided by 2 for two wheeled bots
	CurCM 	= (LeftCM + RightCM) / 2.0

	'Update accumulated Total centimeters traveled
	'This is total linear centemeters travelled for each side along with the total for the center
	TotalLeftCM 		= TotalLeftCM 	+ LeftCM
	TotalRightCM 		= TotalRightCM 	+ RightCM
	TotalCM 			= TotalCM 		+ CurCM

	'Accumulate rotation about axis
	CurThetaRadian 	= (LeftCM - RightCM) / Wheelbase	'Current theta delta, i.e. how much additional angle have we turned
	HeadingRadian		= HeadingRadian + CurThetaRadian	'Update overall heading - Add the delta to the current heading

	'keep the heading within 0 to 360 degrees or 0 to PI2
	'If heading went negative then we add PI2 (360 degrees) back to it
	If HeadingRadian < 0.0 then
		HeadingRadian = HeadingRadian + PI2
	end if

	'If the heading went greater than PI2 (360 degrees) then take 360 off of it
	If HeadingRadian > PI2 then
		HeadingRadian = HeadingRadian - PI2
	end if

	CurThetaDegrees	= CurThetaRadian * RADS		'Convert to degrees for human reading and display

	'Calculate the current X/Y location in our coordinates
	XPos = XPos + (Sin(HeadingRadian) * CurCM)			'This is all done in Radians
	YPos = YPos + (Cos(HeadingRadian) * CurCM)

	Heading = HeadingRadian * RADS						'Put this in degrees for viewing/working with


	'**********************************************
	'Now calc the target distance and heading error we have
	'Get X and Y distance from target
	XDistance = XTarget - XPos
	YDistance = YTarget - YPos

	'Get linear distance to target
	TargetDistance = SQR((XDistance*XDistance)+(YDistance*YDistance))

	'Get the target bearing from our position
	If (XDistance > 0.0) then
		TargetBearing = 90.0 - (ATN(yDistance/xDistance)*RADS)
	elseif (XDistance < 0.0) then
		TargetBearing = -90.0 - (ATN(yDistance/xDistance)*RADS)
	else	'if we are on the X line we have to handle the divide by zero and set the bearing
		If YDistance > 0.0 then
			TargetBearing = 90.0
		else
			TargetBearing = -90.0
		end if
	end if
  
	'Calculate our heading error
	HeadingError = TargetBearing - Heading

	'Wrap the heading error back to either +/- 180 so we do not do a complete turnabout
	If (HeadingError > 180.0) then
		HeadingError = HeadingError - 360.0
	Elseif HeadingError < -180.0 then
		HeadingError = HeadingError + 360.0
	end if
	'**********************************************
end sub