static void diffSteer(Mobile &mobile, float xVel, float yVel)
{
	cli::array<wchar_t, 1>^ left = { 0x0C, 0x00, 0x00, 0x04, 0x01, 0x00, 0x03, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00  };
	cli::array<wchar_t, 1>^ right = { 0x0C, 0x00, 0x00, 0x04, 0x00, 0x00, 0x03, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00 };
	float rVel = sqrt(pow(xVel, 2) + pow(yVel, 2));
	sprintf(rVelVal, "rVel: %.2f", rVel);
	argDrawStringsByIdealPos(rVelVal, 10, 75);
	//float thetaTarget = ((PI / 2) - atan2((xNext - mobile.x), (yNext - mobile.y)))*(180 / PI);
	float thetaTarget = atan2(yVel, xVel)*(180 / PI);
	float deltaTheta = thetaTarget - mobile.theta;
	float W = deltaTheta*(baseR / wheelR);
	sprintf(WVal, "W: %.2f", W);
	argDrawStringsByIdealPos(WVal, 10, 100);
	float rightVel = rVel + W;
	float leftVel = rVel - W;
	sprintf(velRightVal, "rightVel: %.2f", rightVel);
	sprintf(velLeftVal, "leftVel: %.2f", leftVel);
	argDrawStringsByIdealPos(velRightVal, 10, 125);
	argDrawStringsByIdealPos(velLeftVal, 150, 125);

	left[5] = leftVel + 10;
	right[5] = rightVel + 10;

	if (leftVel > 100)
	{
		left[5] = 100;
	}
	else if (leftVel < -100)
	{
		left[5] = -100;
	}

	if (rightVel > 100)
	{
		right[5] = 100;
	}
	else if (rightVel < -100)
	{
		right[5] = -100;
	}

	if (rVel < 4.0) {
		left[5] = 0;
		right[5] = 0;
		NXT->Write(left, 0, 14);
		NXT->Write(right, 0, 14);
		exit(0);
	}

	NXT->Write(left, 0, 14);
	NXT->Write(right, 0, 14);

	velLeftPrev = leftVel;
	velRightPrev = rightVel;
}