robotdrive.cpp

good luck to everyone!

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
/*----------------------------------------------------------------------------*/

#include "RobotDrive.h"

#include "GenericHID.h"
#include "Joystick.h"
#include "Jaguar.h"
#include "Utility.h"
#include "WPIStatus.h"
#include <math.h>

#define max(x, y) (((x) > (y)) ? (x) : (y))

/*
 * Driving functions
 * These functions provide an interface to multiple motors that is used for C programming
 * The Drive(speed, direction) function is the main part of the set that makes it easy
 * to set speeds and direction independently in one call.
 */

/** Constructor for RobotDrive with 2 motors specified with channel numbers.
 * Set up parameters for a two wheel drive system where the
 * left and right motor pwm channels are specified in the call.
 * This call assumes Jaguars for controlling the motors.
 * @param leftMotorChannel The PWM channel number on the default digital module that drives the left motor.
 * @param rightMotorChannel The PWM channel number on the default digital module that drives the right motor.
 * @param sensitivity Effectively sets the turning sensitivity (or turn radius for a given value).
 */
RobotDrive::RobotDrive(UINT32 leftMotorChannel, UINT32 rightMotorChannel,
		float sensitivity)
{
	m_sensitivity = sensitivity;
	m_frontLeftMotor = NULL;
	m_rearLeftMotor = new Jaguar(leftMotorChannel);
	m_frontRightMotor = NULL;
	m_rearRightMotor = new Jaguar(rightMotorChannel);
	for (INT32 i=0; i < kMaxNumberOfMotors; i++)
	{
		m_invertedMotors[i] = 1;
	}
	Drive(0, 0);
	m_deleteSpeedControllers = true;
}

/**
 * Constructor for RobotDrive with 4 motors specified with channel numbers.
 * Set up parameters for a four wheel drive system where all four motor
 * pwm channels are specified in the call.
 * This call assumes Jaguars for controlling the motors.
 * @param frontLeftMotor Front left motor channel number on the default digital module
 * @param rearLeftMotor Rear Left motor channel number on the default digital module
 * @param frontRightMotor Front right motor channel number on the default digital module
 * @param rearRightMotor Rear Right motor channel number on the default digital module
 * @param sensitivity Effectively sets the turning sensitivity (or turn radius for a given value)
 */
RobotDrive::RobotDrive(UINT32 frontLeftMotor, UINT32 rearLeftMotor,
		UINT32 frontRightMotor, UINT32 rearRightMotor, float sensitivity)
{
	m_sensitivity = sensitivity;
	m_rearLeftMotor = new Jaguar(rearLeftMotor);
	m_rearRightMotor = new Jaguar(rearRightMotor);
	m_frontLeftMotor = new Jaguar(frontLeftMotor);
	m_frontRightMotor = new Jaguar(frontRightMotor);
	for (INT32 i=0; i < kMaxNumberOfMotors; i++)
	{
		m_invertedMotors[i] = 1;
	}
	Drive(0, 0);
	m_deleteSpeedControllers = true;
}

/**
 * Constructor for RobotDrive with 2 motors specified as SpeedController objects.
 * The SpeedController version of the constructor enables programs to use the RobotDrive classes with
 * subclasses of the SpeedController objects, for example, versions with ramping or reshaping of
 * the curve to suit motor bias or deadband elimination.
 * @param leftMotor The left SpeedController object used to drive the robot.
 * @param rightMotor the right SpeedController object used to drive the robot.
 * @param sensitivity Effectively sets the turning sensitivity (or turn radius for a given value)
 */
RobotDrive::RobotDrive(SpeedController *leftMotor, SpeedController *rightMotor, float sensitivity)
{
	if (leftMotor == NULL || rightMotor == NULL)
	{
		wpi_fatal(NullParameter);
		m_rearLeftMotor = m_rearRightMotor = NULL;
		return;
	}
	m_frontLeftMotor = NULL;
	m_rearLeftMotor = leftMotor;
	m_frontRightMotor = NULL;
	m_rearRightMotor = rightMotor;
	m_sensitivity = sensitivity;
	for (INT32 i=0; i < kMaxNumberOfMotors; i++)
	{
		m_invertedMotors[i] = 1;
	}
	m_deleteSpeedControllers = false;
}

RobotDrive::RobotDrive(SpeedController &leftMotor, SpeedController &rightMotor, float sensitivity)
{
	m_frontLeftMotor = NULL;
	m_rearLeftMotor = &leftMotor;
	m_frontRightMotor = NULL;
	m_rearRightMotor = &rightMotor;
	m_sensitivity = sensitivity;
	for (INT32 i=0; i < kMaxNumberOfMotors; i++)
	{
		m_invertedMotors[i] = 1;
	}
	m_deleteSpeedControllers = false;
}

/**
 * Constructor for RobotDrive with 4 motors specified as SpeedController objects.
 * Speed controller input version of RobotDrive (see previous comments).
 * @param rearLeftMotor The back left SpeedController object used to drive the robot.
 * @param frontLeftMotor The front left SpeedController object used to drive the robot
 * @param rearRightMotor The back right SpeedController object used to drive the robot.
 * @param frontRightMotor The front right SpeedController object used to drive the robot.
 * @param sensitivity Effectively sets the turning sensitivity (or turn radius for a given value)
 */
RobotDrive::RobotDrive(SpeedController *frontLeftMotor, SpeedController *rearLeftMotor,
						SpeedController *frontRightMotor, SpeedController *rearRightMotor,
						float sensitivity)
{
	if (frontLeftMotor == NULL || rearLeftMotor == NULL || frontRightMotor == NULL || rearRightMotor == NULL)
	{
		wpi_fatal(NullParameter);
		m_frontLeftMotor = m_rearLeftMotor = m_frontRightMotor = m_rearRightMotor = NULL;
		return;
	}
	m_frontLeftMotor = frontLeftMotor;
	m_rearLeftMotor = rearLeftMotor;
	m_frontRightMotor = frontRightMotor;
	m_rearRightMotor = rearRightMotor;
	m_sensitivity = sensitivity;
	for (INT32 i=0; i < kMaxNumberOfMotors; i++)
	{
		m_invertedMotors[i] = 1;
	}
	m_deleteSpeedControllers = false;
}

RobotDrive::RobotDrive(SpeedController &frontLeftMotor, SpeedController &rearLeftMotor,
						SpeedController &frontRightMotor, SpeedController &rearRightMotor,
						float sensitivity)
{
	m_frontLeftMotor = &frontLeftMotor;
	m_rearLeftMotor = &rearLeftMotor;
	m_frontRightMotor = &frontRightMotor;
	m_rearRightMotor = &rearRightMotor;
	m_sensitivity = sensitivity;
	for (INT32 i=0; i < kMaxNumberOfMotors; i++)
	{
		m_invertedMotors[i] = 1;
	}
	m_deleteSpeedControllers = false;
}

/**
 * RobotDrive destructor.
 * Deletes motor objects that were not passed in and created internally only.
 **/
RobotDrive::~RobotDrive()
{
	if (m_deleteSpeedControllers)
	{
		delete m_frontLeftMotor;
		delete m_rearLeftMotor;
		delete m_frontRightMotor;
		delete m_rearRightMotor;
	}
}

/**
 * Drive the motors at "speed" and "curve".
 *
 * The speed and curve are -1.0 to +1.0 values where 0.0 represents stopped and
 * not turning. The algorithm for adding in the direction attempts to provide a constant
 * turn radius for differing speeds.
 *
 * This function sill most likely be used in an autonomous routine.
 *
 * @param speed The forward component of the speed to send to the motors.
 * @param curve The rate of turn, constant for different forward speeds.
 */
void RobotDrive::Drive(float speed, float curve)
{
	float leftSpeed, rightSpeed;

	if (curve < 0)
	{
		float value = log(-curve);
		float ratio = (value - m_sensitivity)/(value + m_sensitivity);
		if (ratio == 0) ratio =.0000000001;
		leftSpeed = speed / ratio;
		rightSpeed = speed;
	}
	else if (curve > 0)
	{
		float value = log(curve);
		float ratio = (value - m_sensitivity)/(value + m_sensitivity);
		if (ratio == 0) ratio =.0000000001;
		leftSpeed = speed;
		rightSpeed = speed / ratio;
	}
	else
	{
		leftSpeed = speed;
		rightSpeed = speed;
	}
	SetLeftRightMotorSpeeds(leftSpeed, rightSpeed);
}

/**
 * Provide tank steering using the stored robot configuration.
 * Drive the robot using two joystick inputs. The Y-axis will be selected from
 * each Joystick object.
 * @param leftStick The joystick to control the left side of the robot.
 * @param rightStick The joystick to control the right side of the robot.
 */
void RobotDrive::TankDrive(GenericHID *leftStick, GenericHID *rightStick)
{
	if (leftStick == NULL || rightStick == NULL)
	{
		wpi_fatal(NullParameter);
		return;
	}
	TankDrive(leftStick->GetY(), rightStick->GetY());
}

void RobotDrive::TankDrive(GenericHID &leftStick, GenericHID &rightStick)
{
	TankDrive(leftStick.GetY(), rightStick.GetY());
}

/**
 * Provide tank steering using the stored robot configuration.
 * This function lets you pick the axis to be used on each Joystick object for the left
 * and right sides of the robot.
 * @param leftStick The Joystick object to use for the left side of the robot.
 * @param leftAxis The axis to select on the left side Joystick object.
 * @param rightStick The Joystick object to use for the right side of the robot.
 * @param rightAxis The axis to select on the right side Joystick object.
 */
void RobotDrive::TankDrive(GenericHID *leftStick, UINT32 leftAxis,
		GenericHID *rightStick, UINT32 rightAxis)
{
	if (leftStick == NULL || rightStick == NULL)
	{
		wpi_fatal(NullParameter);
		return;
	}
	TankDrive(leftStick->GetRawAxis(leftAxis), rightStick->GetRawAxis(rightAxis));
}

void RobotDrive::TankDrive(GenericHID &leftStick, UINT32 leftAxis,