robotdrive.cpp

good luck to everyone!

		GenericHID &rightStick, UINT32 rightAxis)
{
	TankDrive(leftStick.GetRawAxis(leftAxis), rightStick.GetRawAxis(rightAxis));
}


/**
 * Provide tank steering using the stored robot configuration.
 * This function lets you directly provide joystick values from any source.
 * @param leftValue The value of the left stick.
 * @param rightValue The value of the right stick.
 */
void RobotDrive::TankDrive(float leftValue, float rightValue)
{
	// square the inputs (while preserving the sign) to increase fine control while permitting full power
	leftValue = Limit(leftValue);
	rightValue = Limit(rightValue);
	if (leftValue >= 0.0)
	{
		leftValue = (leftValue * leftValue);
	}
	else
	{
		leftValue = -(leftValue * leftValue);
	}
	if (rightValue >= 0.0)
	{
		rightValue = (rightValue * rightValue);
	}
	else
	{
		rightValue = -(rightValue * rightValue);
	}

	SetLeftRightMotorSpeeds(leftValue, rightValue);
}

/**
 * Arcade drive implements single stick driving.
 * Given a single Joystick, the class assumes the Y axis for the move value and the X axis
 * for the rotate value.
 * (Should add more information here regarding the way that arcade drive works.)
 * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
 * for forwards/backwards and the X-axis will be selected for rotation rate.
 * @param squaredInputs If true, the sensitivity will be increased for small values
 */
void RobotDrive::ArcadeDrive(GenericHID *stick, bool squaredInputs)
{
	// simply call the full-featured ArcadeDrive with the appropriate values
	ArcadeDrive(stick->GetY(), stick->GetX(), stick->GetTrigger());
}

/**
 * Arcade drive implements single stick driving.
 * Given a single Joystick, the class assumes the Y axis for the move value and the X axis
 * for the rotate value.
 * (Should add more information here regarding the way that arcade drive works.)
 * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
 * for forwards/backwards and the X-axis will be selected for rotation rate.
 * @param squaredInputs If true, the sensitivity will be increased for small values
 */
void RobotDrive::ArcadeDrive(GenericHID &stick, bool squaredInputs)
{
	// simply call the full-featured ArcadeDrive with the appropriate values
	ArcadeDrive(stick.GetY(), stick.GetX(), stick.GetTrigger());
}

/**
 * Arcade drive implements single stick driving.
 * Given two joystick instances and two axis, compute the values to send to either two
 * or four motors.
 * @param moveStick The Joystick object that represents the forward/backward direction
 * @param moveAxis The axis on the moveStick object to use for fowards/backwards (typically Y_AXIS)
 * @param rotateStick The Joystick object that represents the rotation value
 * @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically X_AXIS)
 * @param squaredInputs Setting this parameter to true increases the sensitivity at lower speeds
 */
void RobotDrive::ArcadeDrive(GenericHID* moveStick, UINT32 moveAxis,
								GenericHID* rotateStick, UINT32 rotateAxis,
								bool squaredInputs)
{
	float moveValue = moveStick->GetRawAxis(moveAxis);
	float rotateValue = rotateStick->GetRawAxis(rotateAxis);

	ArcadeDrive(moveValue, rotateValue, squaredInputs);
}

/**
 * Arcade drive implements single stick driving.
 * Given two joystick instances and two axis, compute the values to send to either two
 * or four motors.
 * @param moveStick The Joystick object that represents the forward/backward direction
 * @param moveAxis The axis on the moveStick object to use for fowards/backwards (typically Y_AXIS)
 * @param rotateStick The Joystick object that represents the rotation value
 * @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically X_AXIS)
 * @param squaredInputs Setting this parameter to true increases the sensitivity at lower speeds
 */

void RobotDrive::ArcadeDrive(GenericHID &moveStick, UINT32 moveAxis,
								GenericHID &rotateStick, UINT32 rotateAxis,
								bool squaredInputs)
{
	float moveValue = moveStick.GetRawAxis(moveAxis);
	float rotateValue = rotateStick.GetRawAxis(rotateAxis);

	ArcadeDrive(moveValue, rotateValue, squaredInputs);
}

/**
 * Arcade drive implements single stick driving.
 * This function lets you directly provide joystick values from any source.
 * @param moveValue The value to use for fowards/backwards
 * @param rotateValue The value to use for the rotate right/left
 * @param squaredInputs If set, increases the sensitivity at low speeds
 */
void RobotDrive::ArcadeDrive(float moveValue, float rotateValue, bool squaredInputs)
{
	// local variables to hold the computed PWM values for the motors
	float leftMotorSpeed;
	float rightMotorSpeed;

	moveValue = Limit(moveValue);
	rotateValue = Limit(rotateValue);

	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);
		}
	}
	SetLeftRightMotorSpeeds(leftMotorSpeed, rightMotorSpeed);

}

/**
 * Holonomic Drive class for Mecanum wheeled robots.
 *
 * Experimental class for driving with Mecanum wheeled robots. There are 4 wheels
 * on the robot, arranged so that the front and back wheels are toed in 45 degrees.
 *
 * @param magnitude The speed that the robot should drive in a given direction.
 * @param direction The direction the robot should drive. The direction and maginitute are
 * independent of the rotation rate.
 * @param rotation The rate of rotation for the robot that is completely independent of
 * the magnitute or direction.
 */
void RobotDrive::HolonomicDrive(float magnitude, float direction, float rotation)
{
	float frontLeftSpeed, rearLeftSpeed, frontRightSpeed, rearRightSpeed;
	magnitude = Limit(magnitude);
	float cosD = cos((float)(direction + 45.0) * 3.14159 / 180.0);
	float sinD = cos((float)(direction - 45.0) * 3.14159 / 180.0);
	frontLeftSpeed = Limit((float)(sinD * (float)magnitude + rotation));
	rearLeftSpeed = Limit((float)(cosD * (float)magnitude + rotation));
	frontRightSpeed = Limit((float)(cosD * (float)magnitude - rotation));
	rearRightSpeed = Limit((float)(sinD * (float)magnitude - rotation));

	m_frontLeftMotor->Set(frontLeftSpeed * m_invertedMotors[kFrontLeftMotor]);
	m_frontRightMotor->Set(frontRightSpeed * m_invertedMotors[kFrontRightMotor]);
	m_rearLeftMotor->Set(rearLeftSpeed * m_invertedMotors[kRearLeftMotor]);
	m_rearRightMotor->Set(rearRightSpeed * m_invertedMotors[kRearRightMotor]);
}

/** Set the speed of the right and left motors.
 * This is used once an appropriate drive setup function is called such as
 * TwoWheelDrive(). The motors are set to "leftSpeed" and "rightSpeed"
 * and includes flipping the direction of one side for opposing motors.
 * @param leftSpeed The speed to send to the left side of the robot.
 * @param rightSpeed The speed to send to the right side of the robot.
 */
void RobotDrive::SetLeftRightMotorSpeeds(float leftSpeed, float rightSpeed)
{
	wpi_assert(m_rearLeftMotor != NULL && m_rearRightMotor != NULL);

	leftSpeed = Limit(leftSpeed);
	rightSpeed = Limit(rightSpeed);

	if (m_frontLeftMotor != NULL)
		m_frontLeftMotor->Set(Limit(leftSpeed) * m_invertedMotors[kFrontLeftMotor]);
	m_rearLeftMotor->Set(Limit(leftSpeed) * m_invertedMotors[kRearLeftMotor]);

	if (m_frontRightMotor != NULL)
		m_frontRightMotor->Set(-Limit(rightSpeed) * m_invertedMotors[kFrontRightMotor]);
	m_rearRightMotor->Set(-Limit(rightSpeed) * m_invertedMotors[kRearRightMotor]);
}

/**
 * Limit motor values to the -1.0 to +1.0 range.
 */
float RobotDrive::Limit(float num)
{
	if (num > 1.0)
	{
		return 1.0;
	}
	if (num < -1.0)
	{
		return -1.0;
	}
	return num;
}

/*
 * Invert a motor direction.
 * This is used when a motor should run in the opposite direction as the drive
 * code would normally run it. Motors that are direct drive would be inverted, the
 * Drive code assumes that the motors are geared with one reversal.
 * @param motor The motor index to invert.
 * @param isInverted True if the motor should be inverted when operated.
 */
void RobotDrive::SetInvertedMotor(MotorType motor, bool isInverted)
{
	if (motor < 0 || motor > 3)
	{
		wpi_fatal(InvalidMotorIndex);
		return;
	}
	m_invertedMotors[motor] = isInverted ? -1 : 1;
}