DriveControl.cpp

#include "DriveControl.h"

float DriveControl::Limit(float num)
{
	if (num > 1.0)
	{
		return 1.0;
	}
	if (num < -1.0)
	{
		return -1.0;
	}
	return num;
}

void DriveControl::ArcadeToTank(float x, float y, float & left, float & right, bool squaredInputs) {
	float leftMotorSpeed;
	float rightMotorSpeed;

	float moveValue = Limit(y);
	float rotateValue = Limit(x);

	if (squaredInputs) {
		// square the inputs (while preserving the sign) to increase fine control while permitting full power
		if (moveValue >= 0.0) {
			moveValue = (moveValue * moveValue);
		} else {
			moveValue = -(moveValue * moveValue);
		}
		if (rotateValue >= 0.0) {
			rotateValue = (rotateValue * rotateValue);
		} else {
			rotateValue = -(rotateValue * rotateValue);
		}
	}

	if (moveValue > 0.0) {
		if (rotateValue > 0.0) {
			leftMotorSpeed = moveValue - rotateValue;
			rightMotorSpeed = max(moveValue, rotateValue);
		} else {
			leftMotorSpeed = max(moveValue, -rotateValue);
			rightMotorSpeed = moveValue + rotateValue;
		}
	} else {
		if (rotateValue > 0.0) {
			leftMotorSpeed = -max(-moveValue, rotateValue);
			rightMotorSpeed = moveValue + rotateValue;
		} else {
			leftMotorSpeed = moveValue - rotateValue;
			rightMotorSpeed = -max(-moveValue, -rotateValue);
		}
	}
	
	// give the mapped outputs back to the client.
	left = leftMotorSpeed;
	right = rightMotorSpeed;
}