Quasics2015Code/src/DriveTrain/DriveTrainSmoothing.cpp
void TrimJoystickValuesToPower(bool useLinearScaling, float leftIn,
		float rightIn, float& leftOut, float& rightOut) {
	// bring inputs to between -1 and 1
	leftIn = min(max(leftIn, -1.0f), +1.0f);
	rightIn = min(max(rightIn, -1.0f), +1.0f);

	// Conversion from input to table
	const int indexOffset = sizeOfMultiplierDataTable / 2; // e.g., 41 / 2 ==> 20,
														   //since they are integers

	/* Scaling the values to an integer multiple of (roughly) half our table
	 size, so that we can convert them into an index into the table in the
	 next step.  (For example, -1 ==> -20, +1 ==> +20, etc.)

	 Note that this will always round down (e.g. .99 * 20 ==> 19.8 when scaled,
	 but we will round that down to 19, which gives us safe values for use in
	 figuring out the index).
	 */
	const int leftScaled = int(leftIn * indexOffset);
	const int rightScaled = int(rightIn * indexOffset);

	// OK, take the scaled size and do the conversion.
	const int leftIndex = leftScaled + indexOffset;
	const int rightIndex = rightScaled + indexOffset;

	//Set default multipliers (final multipliers if not using interpolation)
	float leftMultiplier = multiplierData[leftIndex].leftMultiplier;
	float rightMultiplier = multiplierData[rightIndex].rightMultiplier;

	if (useLinearScaling) {
		if (leftIndex == sizeOfMultiplierDataTable - 1) {
			//Since we can't pull a greater number, just return the default for max
		} else {
			// Define variables for interpolation
			const float x0 = leftScaled;	// previously defined floor
			const float y0 = multiplierData[leftIndex].leftMultiplier; //get Y value for x0
			const float x1 = leftScaled + joystickStep; //grab next value from data table
			const float y1 = multiplierData[leftIndex + 1].leftMultiplier; //evaluate for x1
			const float x = leftIn * indexOffset; //non-integer left scaled value

			leftMultiplier = LinearInterpolation(x0, y0, x1, y1, x); //run equation
		}

		if (rightIndex == sizeOfMultiplierDataTable - 1) {
			//Since we can't pull a greater number, just return the default for max
		} else {
			// Define variables for interpolation
			const float x0 = rightScaled;	// previously defined floor
			const float y0 = multiplierData[rightIndex].rightMultiplier; //get Y value for x0
			const float x1 = rightScaled + joystickStep; //grab next value from data table
			const float y1 = multiplierData[rightIndex + 1].rightMultiplier; //evaluate for x1
			const float x = rightIn * indexOffset; //non-integer left scaled value

			rightMultiplier = LinearInterpolation(x0, y0, x1, y1, x); //run equation
		}
	}

	//output trimmed values
	leftOut = leftMultiplier * leftIn;
	rightOut = rightMultiplier * rightIn;
}
/* Base function for trim calculation
 * Can either use stepped scaling or interpolated scaling, depending on the bool
 */
Base function for trim calculation
Can either use stepped scaling or interpolated scaling, depending on the bool
Base function for trim calculation Can either use stepped scaling or interpolated scaling depending on the bool
void TrimJoystickValuesToPower(bool useLinearScaling, float leftIn,
		float rightIn, float& leftOut, float& rightOut) {
	leftIn = min(max(leftIn, -1.0f), +1.0f);
	rightIn = min(max(rightIn, -1.0f), +1.0f);
	const int indexOffset = sizeOfMultiplierDataTable / 2; 
	const int leftScaled = int(leftIn * indexOffset);
	const int rightScaled = int(rightIn * indexOffset);
	const int leftIndex = leftScaled + indexOffset;
	const int rightIndex = rightScaled + indexOffset;
	float leftMultiplier = multiplierData[leftIndex].leftMultiplier;
	float rightMultiplier = multiplierData[rightIndex].rightMultiplier;
	if (useLinearScaling) {
		if (leftIndex == sizeOfMultiplierDataTable - 1) {
		} else {
			const float x0 = leftScaled;	
			const float y0 = multiplierData[leftIndex].leftMultiplier; 
			const float x1 = leftScaled + joystickStep; 
			const float y1 = multiplierData[leftIndex + 1].leftMultiplier; 
			const float x = leftIn * indexOffset; 
			leftMultiplier = LinearInterpolation(x0, y0, x1, y1, x); 
		}
		if (rightIndex == sizeOfMultiplierDataTable - 1) {
		} else {
			const float x0 = rightScaled;	
			const float y0 = multiplierData[rightIndex].rightMultiplier; 
			const float x1 = rightScaled + joystickStep; 
			const float y1 = multiplierData[rightIndex + 1].rightMultiplier; 
			const float x = rightIn * indexOffset; 
			rightMultiplier = LinearInterpolation(x0, y0, x1, y1, x); 
		}
	}
	leftOut = leftMultiplier * leftIn;
	rightOut = rightMultiplier * rightIn;
}
void TrimJoystickValuesToPower ( bool useLinearScaling , float leftIn , float rightIn , float & leftOut , float & rightOut ) { leftIn = min ( max ( leftIn , -1.0f ) , +1.0f ) ; rightIn = min ( max ( rightIn , -1.0f ) , +1.0f ) ; const int indexOffset = sizeOfMultiplierDataTable / 2 ; const int leftScaled = int ( leftIn * indexOffset ) ; const int rightScaled = int ( rightIn * indexOffset ) ; const int leftIndex = leftScaled + indexOffset ; const int rightIndex = rightScaled + indexOffset ; float leftMultiplier = multiplierData [ leftIndex ] . leftMultiplier ; float rightMultiplier = multiplierData [ rightIndex ] . rightMultiplier ; if ( useLinearScaling ) { if ( leftIndex == sizeOfMultiplierDataTable - 1 ) { } else { const float x0 = leftScaled ; const float y0 = multiplierData [ leftIndex ] . leftMultiplier ; const float x1 = leftScaled + joystickStep ; const float y1 = multiplierData [ leftIndex + 1 ] . leftMultiplier ; const float x = leftIn * indexOffset ; leftMultiplier = LinearInterpolation ( x0 , y0 , x1 , y1 , x ) ; } if ( rightIndex == sizeOfMultiplierDataTable - 1 ) { } else { const float x0 = rightScaled ; const float y0 = multiplierData [ rightIndex ] . rightMultiplier ; const float x1 = rightScaled + joystickStep ; const float y1 = multiplierData [ rightIndex + 1 ] . rightMultiplier ; const float x = rightIn * indexOffset ; rightMultiplier = LinearInterpolation ( x0 , y0 , x1 , y1 , x ) ; } } leftOut = leftMultiplier * leftIn ; rightOut = rightMultiplier * rightIn ; }
Base function for trim calculation
 Can either use stepped scaling or interpolated scaling, depending on the bool
Base function for trim calculation Can either use stepped scaling or interpolated scaling depending on the bool
// bring inputs to between -1 and 1 // Conversion from input to table // e.g., 41 / 2 ==> 20, //since they are integers /* Scaling the values to an integer multiple of (roughly) half our table
	 size, so that we can convert them into an index into the table in the
	 next step.  (For example, -1 ==> -20, +1 ==> +20, etc.)

	 Note that this will always round down (e.g. .99 * 20 ==> 19.8 when scaled,
	 but we will round that down to 19, which gives us safe values for use in
	 figuring out the index).
	 */ // OK, take the scaled size and do the conversion. //Set default multipliers (final multipliers if not using interpolation) //Since we can't pull a greater number, just return the default for max // Define variables for interpolation // previously defined floor //get Y value for x0 //grab next value from data table //evaluate for x1 //non-integer left scaled value //run equation //Since we can't pull a greater number, just return the default for max // Define variables for interpolation // previously defined floor //get Y value for x0 //grab next value from data table //evaluate for x1 //non-integer left scaled value //run equation //output trimmed values