void RobotDrive3::ArcadeDrive(double moveValue,double rotateValue){
double leftMotorSpeed,rightMotorSpeed;
//prevent motor values from being over 1 or under -1
moveValue = Limit(moveValue);
rotateValue = Limit(rotateValue);
//cubed rotation
rotateValue *= rotateValue * rotateValue;
//set left and right motor speed variables
if(moveValue > 0){
if(rotateValue > 0){
leftMotorSpeed = moveValue - rotateValue;
rightMotorSpeed = max(rotateValue,moveValue);
}else{
rightMotorSpeed = moveValue + rotateValue;
leftMotorSpeed = max(-rotateValue, moveValue);
}
}else{
if(rotateValue > 0){
leftMotorSpeed = min(moveValue, -rotateValue);
rightMotorSpeed = moveValue + rotateValue;
}else{
rightMotorSpeed = min(moveValue, rotateValue);
leftMotorSpeed = moveValue - rotateValue;
}
}
SetLeftRightMotorOutputs(leftMotorSpeed,rightMotorSpeed);
}
/**
* 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, bool squaredInputs)
{
static bool reported = false;
if (!reported)
{
nUsageReporting::report(nUsageReporting::kResourceType_RobotDrive, GetNumMotors(), nUsageReporting::kRobotDrive_Tank);
reported = true;
}
// square the inputs (while preserving the sign) to increase fine control while permitting full power
leftValue = Limit(leftValue);
rightValue = Limit(rightValue);
if(squaredInputs)
{
if (leftValue >= 0.0)
{
leftValue = (leftValue * leftValue);
}
else
{
leftValue = -(leftValue * leftValue);
}
if (rightValue >= 0.0)
{
rightValue = (rightValue * rightValue);
}
else
{
rightValue = -(rightValue * rightValue);
}
}
SetLeftRightMotorOutputs(leftValue, rightValue);
}
/**
* 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 will most likely be used in an autonomous routine.
*
* @param outputMagnitude The forward component of the output magnitude to send to the motors.
* @param curve The rate of turn, constant for different forward speeds.
*/
void RobotDrive::Drive(float outputMagnitude, float curve)
{
float leftOutput, rightOutput;
static bool reported = false;
if (!reported)
{
reported = true;
}
if (curve < 0)
{
float value = log(-curve);
float ratio = (value - m_sensitivity)/(value + m_sensitivity);
if (ratio == 0) ratio =.0000000001;
leftOutput = outputMagnitude / ratio;
rightOutput = outputMagnitude;
}
else if (curve > 0)
{
float value = log(curve);
float ratio = (value - m_sensitivity)/(value + m_sensitivity);
if (ratio == 0) ratio =.0000000001;
leftOutput = outputMagnitude;
rightOutput = outputMagnitude / ratio;
}
else
{
leftOutput = outputMagnitude;
rightOutput = outputMagnitude;
}
SetLeftRightMotorOutputs(leftOutput, rightOutput);
}
/**
* 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 will most likely be used in an autonomous routine.
*
* @param outputMagnitude The forward component of the output magnitude to send to the motors.
* @param curve The rate of turn, constant for different forward speeds.
*/
void RobotDrive::Drive(float outputMagnitude, float curve)
{
float leftOutput, rightOutput;
static bool reported = false;
if (!reported)
{
nUsageReporting::report(nUsageReporting::kResourceType_RobotDrive, GetNumMotors(), nUsageReporting::kRobotDrive_ArcadeRatioCurve);
reported = true;
}
if (curve < 0)
{
float value = log(-curve);
float ratio = (value - m_sensitivity)/(value + m_sensitivity);
if (ratio == 0) ratio =.0000000001;
leftOutput = outputMagnitude / ratio;
rightOutput = outputMagnitude;
}
else if (curve > 0)
{
float value = log(curve);
float ratio = (value - m_sensitivity)/(value + m_sensitivity);
if (ratio == 0) ratio =.0000000001;
leftOutput = outputMagnitude;
rightOutput = outputMagnitude / ratio;
}
else
{
leftOutput = outputMagnitude;
rightOutput = outputMagnitude;
}
SetLeftRightMotorOutputs(leftOutput, rightOutput);
}
/**
* 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 Talons for controlling the motors.
* @param frontLeftMotor Front left motor channel number. 0-9 are on-board,
* 10-19 are on the MXP port
* @param rearLeftMotor Rear Left motor channel number. 0-9 are on-board, 10-19
* are on the MXP port
* @param frontRightMotor Front right motor channel number. 0-9 are on-board,
* 10-19 are on the MXP port
* @param rearRightMotor Rear Right motor channel number. 0-9 are on-board,
* 10-19 are on the MXP port
*/
RobotDrive::RobotDrive(uint32_t frontLeftMotor, uint32_t rearLeftMotor,
uint32_t frontRightMotor, uint32_t rearRightMotor) {
InitRobotDrive();
m_rearLeftMotor = std::make_shared<Talon>(rearLeftMotor);
m_rearRightMotor = std::make_shared<Talon>(rearRightMotor);
m_frontLeftMotor = std::make_shared<Talon>(frontLeftMotor);
m_frontRightMotor = std::make_shared<Talon>(frontRightMotor);
SetLeftRightMotorOutputs(0.0, 0.0);
}
/** 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.
*/
RobotDrive::RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel)
{
InitRobotDrive();
m_rearLeftMotor = new Jaguar(leftMotorChannel);
m_rearRightMotor = new Jaguar(rightMotorChannel);
for (int32_t i=0; i < kMaxNumberOfMotors; i++)
{
m_invertedMotors[i] = 1;
}
SetLeftRightMotorOutputs(0.0, 0.0);
m_deleteSpeedControllers = true;
}
RobotDrive3::RobotDrive3(SpeedController &frontMotor,
SpeedController &rearLeftMotor,
SpeedController &rearRightMotor)
: RobotDrive(rearLeftMotor, rearRightMotor)
{
m_frontLeftMotor = &frontMotor;
SetLeftRightMotorOutputs(0.0, 0.0);
m_safetyHelper->SetSafetyEnabled(false);
m_lastSpeeds[kFrontLeftMotor] = 0.0;
m_lastSpeeds[kFrontRightMotor] = 0.0; // not used
m_lastSpeeds[kRearLeftMotor] = 0.0;
m_lastSpeeds[kRearRightMotor] = 0.0;
}
RobotDrive3::RobotDrive3(uint32_t frontMotorChannel,
uint32_t rearLeftMotorChannel,
uint32_t rearRightMotorChannel)
: RobotDrive(rearLeftMotorChannel, rearRightMotorChannel)
{
m_frontLeftMotor = new Jaguar(frontMotorChannel);
SetLeftRightMotorOutputs(0.0, 0.0);
m_safetyHelper->SetSafetyEnabled(false);
m_lastSpeeds[kFrontLeftMotor] = 0.0;
m_lastSpeeds[kFrontRightMotor] = 0.0; // not used
m_lastSpeeds[kRearLeftMotor] = 0.0;
m_lastSpeeds[kRearRightMotor] = 0.0;
}
/**
* 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) {
static bool reported = false;
if (!reported) {
HALReport(HALUsageReporting::kResourceType_RobotDrive, GetNumMotors(),
HALUsageReporting::kRobotDrive_ArcadeStandard);
reported = true;
}
// local variables to hold the computed PWM values for the motors
float leftMotorOutput;
float rightMotorOutput;
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) {
leftMotorOutput = moveValue - rotateValue;
rightMotorOutput = std::max(moveValue, rotateValue);
} else {
leftMotorOutput = std::max(moveValue, -rotateValue);
rightMotorOutput = moveValue + rotateValue;
}
} else {
if (rotateValue > 0.0) {
leftMotorOutput = -std::max(-moveValue, rotateValue);
rightMotorOutput = moveValue + rotateValue;
} else {
leftMotorOutput = moveValue - rotateValue;
rightMotorOutput = -std::max(-moveValue, -rotateValue);
}
}
SetLeftRightMotorOutputs(leftMotorOutput, rightMotorOutput);
}
/**
* 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
*/
RobotDrive::RobotDrive(UINT32 frontLeftMotor, UINT32 rearLeftMotor,
UINT32 frontRightMotor, UINT32 rearRightMotor)
{
InitRobotDrive();
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;
}
SetLeftRightMotorOutputs(0.0, 0.0);
m_deleteSpeedControllers = true;
}
RobotDrive3::RobotDrive3(SpeedController *frontMotor,
SpeedController *rearLeftMotor,
SpeedController *rearRightMotor)
: RobotDrive(rearLeftMotor, rearRightMotor)
{
if (frontMotor == NULL)
{
wpi_setWPIError(NullParameter);
m_rearLeftMotor = m_rearRightMotor = NULL;
return;
}
m_frontLeftMotor = frontMotor;
SetLeftRightMotorOutputs(0.0, 0.0);
m_safetyHelper->SetSafetyEnabled(false);
m_lastSpeeds[kFrontLeftMotor] = 0.0;
m_lastSpeeds[kFrontRightMotor] = 0.0; // not used
m_lastSpeeds[kRearLeftMotor] = 0.0;
m_lastSpeeds[kRearRightMotor] = 0.0;
}
/**
* 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 Talons for controlling the motors.
* @param leftMotorChannel The PWM channel number that drives the left motor.
* 0-9 are on-board, 10-19 are on the MXP port
* @param rightMotorChannel The PWM channel number that drives the right motor.
* 0-9 are on-board, 10-19 are on the MXP port
*/
RobotDrive::RobotDrive(uint32_t leftMotorChannel, uint32_t rightMotorChannel) {
InitRobotDrive();
m_rearLeftMotor = std::make_shared<Talon>(leftMotorChannel);
m_rearRightMotor = std::make_shared<Talon>(rightMotorChannel);
SetLeftRightMotorOutputs(0.0, 0.0);
}
