WaitUntilDistance for RobotC AutonSystem

WaitUntilDistance.c

void AutoBaseWaitUntilTickCrossed(int wantedLeft, int wantedRight, int maxTime = 5000)
{
	wait1Msec(AutoDriveBase.loopRate * 2);

	bool leftDirection = (SensorValue[AutoDriveBase.leftEn] < wantedLeft);
	bool rightDirection = (SensorValue[AutoDriveBase.leftEn] < wantedRight);

	bool isLeftCompleted = false;
	bool isRightCompleted = false;
	bool isCompleted = false;

	int emergencyCount = 0;
	while(!isCompleted && emergencyCount < abs(maxTime))
	{

		isLeftCompleted = leftDirection ? (SensorValue[AutoDriveBase.leftEn] > wantedLeft) : (SensorValue[AutoDriveBase.leftEn] < wantedLeft);
		isRightCompleted = rightDirection ? (SensorValue[AutoDriveBase.rightEn] > wantedRight) : (SensorValue[AutoDriveBase.rightEn] < wantedRight);
		isCompleted = (isLeftCompleted && isRightCompleted);

		emergencyCount += AutoDriveBase.loopRate;
		wait1Msec(AutoDriveBase.loopRate);
	}
}




void AutoBaseWaitUntilDistance(float waitInch, int maxTime = 5000)
{
	int wantedLeft = AutoDriveBase.lastWantedLeft + (waitInch / AutoDriveBase.wheelCircumference) * 360;
	int wantedRight = AutoDriveBase.lastWantedRight + (waitInch / AutoDriveBase.wheelCircumference) * 360;

	bool exit;
	if(waitInch > 0)
	{
		if(SensorValue[AutoDriveBase.leftEn] > wantedLeft) exit = true;
		if(SensorValue[AutoDriveBase.rightEn] > wantedRight) exit = true;
	}
	else
	{
		if(SensorValue[AutoDriveBase.leftEn] < wantedLeft) exit = true;
		if(SensorValue[AutoDriveBase.rightEn] < wantedRight) exit = true;
	}

	if(!exit)
	{
		AutoBaseWaitUntilTickCrossed(wantedLeft, wantedRight, maxTime);
	}
}