RobotDrive.cpp

/*----------------------------------------------------------------------------*/
/* 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 "CANJaguar.h"
#include "GenericHID.h"
#include "Joystick.h"
#include "Jaguar.h"
#include "Utility.h"
#include "WPIErrors.h"
#include <math.h>

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

const INT32 RobotDrive::kMaxNumberOfMotors;

/*
 * 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.
 */

void RobotDrive::InitRobotDrive() {
        m_frontLeftMotor = NULL;
        m_frontRightMotor = NULL;
        m_rearRightMotor = NULL;
        m_rearLeftMotor = NULL;
        m_sensitivity = 0.5;
        m_maxOutput = 1.0;
        m_safetyHelper = new MotorSafetyHelper(this);
        m_safetyHelper->SetSafetyEnabled(true);
}

RobotDrive::RobotDrive(UINT32 leftMotorChannel, UINT32 rightMotorChannel)
{
        InitRobotDrive();
        m_rearLeftMotor = new Jaguar(leftMotorChannel);
        m_rearRightMotor = new Jaguar(rightMotorChannel);
        for (INT32 i=0; i < kMaxNumberOfMotors; i++)
        {
                m_invertedMotors[i] = 1;
        }
        Drive(0, 0);
        m_deleteSpeedControllers = true;
}

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;
        }
        Drive(0, 0);
        m_deleteSpeedControllers = true;
}

RobotDrive::RobotDrive(SpeedController *leftMotor, SpeedController *rightMotor)
{
        InitRobotDrive();
        if (leftMotor == NULL || rightMotor == NULL)
        {
                wpi_setWPIError(NullParameter);
                m_rearLeftMotor = m_rearRightMotor = NULL;
                return;
        }
        m_rearLeftMotor = leftMotor;
        m_rearRightMotor = rightMotor;
        for (INT32 i=0; i < kMaxNumberOfMotors; i++)
        {
                m_invertedMotors[i] = 1;
        }
        m_deleteSpeedControllers = false;
}

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

RobotDrive::RobotDrive(SpeedController *frontLeftMotor, SpeedController *rearLeftMotor,
                                                SpeedController *frontRightMotor, SpeedController *rearRightMotor)
{
        InitRobotDrive();
        if (frontLeftMotor == NULL || rearLeftMotor == NULL || frontRightMotor == NULL || rearRightMotor == NULL)
        {
                wpi_setWPIError(NullParameter);
                return;
        }
        m_frontLeftMotor = frontLeftMotor;
        m_rearLeftMotor = rearLeftMotor;
        m_frontRightMotor = frontRightMotor;
        m_rearRightMotor = rearRightMotor;
        for (INT32 i=0; i < kMaxNumberOfMotors; i++)
        {
                m_invertedMotors[i] = 1;
        }
        m_deleteSpeedControllers = false;
}

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

RobotDrive::~RobotDrive()
{
        if (m_deleteSpeedControllers)
        {
                delete m_frontLeftMotor;
                delete m_rearLeftMotor;
                delete m_frontRightMotor;
                delete m_rearRightMotor;
        }
        delete m_safetyHelper;
}

void RobotDrive::Drive(float outputMagnitude, float curve)
{
        float leftOutput, rightOutput;

        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);
}

void RobotDrive::TankDrive(GenericHID *leftStick, GenericHID *rightStick)
{
        if (leftStick == NULL || rightStick == NULL)
        {
                wpi_setWPIError(NullParameter);
                return;
        }
        TankDrive(leftStick->GetY(), rightStick->GetY());
}

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

void RobotDrive::TankDrive(GenericHID *leftStick, UINT32 leftAxis,
                GenericHID *rightStick, UINT32 rightAxis)
{
        if (leftStick == NULL || rightStick == NULL)
        {
                wpi_setWPIError(NullParameter);
                return;
        }
        TankDrive(leftStick->GetRawAxis(leftAxis), rightStick->GetRawAxis(rightAxis));
}

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


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);
        }

        SetLeftRightMotorOutputs(leftValue, rightValue);
}

void RobotDrive::ArcadeDrive(GenericHID *stick, bool squaredInputs)
{
        // simply call the full-featured ArcadeDrive with the appropriate values
        ArcadeDrive(stick->GetY(), stick->GetX(), stick->GetTrigger());
}

void RobotDrive::ArcadeDrive(GenericHID &stick, bool squaredInputs)
{
        // simply call the full-featured ArcadeDrive with the appropriate values
        ArcadeDrive(stick.GetY(), stick.GetX(), stick.GetTrigger());
}

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);
}

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);
}

void RobotDrive::ArcadeDrive(float moveValue, float rotateValue, bool squaredInputs)
{
        // 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 = max(moveValue, rotateValue);
                }
                else
                {
                        leftMotorOutput = max(moveValue, -rotateValue);
                        rightMotorOutput = moveValue + rotateValue;
                }
        }
        else
        {
                if (rotateValue > 0.0)
                {
                        leftMotorOutput = - max(-moveValue, rotateValue);
                        rightMotorOutput = moveValue + rotateValue;
                }
                else
                {
                        leftMotorOutput = moveValue - rotateValue;
                        rightMotorOutput = - max(-moveValue, -rotateValue);
                }
        }
        SetLeftRightMotorOutputs(leftMotorOutput, rightMotorOutput);
}

void RobotDrive::MecanumDrive_Cartesian(float x, float y, float rotation, float gyroAngle)
{
        double xIn = x;
        double yIn = y;
        // Negate y for the joystick.
        yIn = -yIn;
        // Compenstate for gyro angle.
        RotateVector(xIn, yIn, gyroAngle);

        double wheelSpeeds[kMaxNumberOfMotors];
        wheelSpeeds[kFrontLeftMotor] = xIn + yIn + rotation;
        wheelSpeeds[kFrontRightMotor] = -xIn + yIn - rotation;
        wheelSpeeds[kRearLeftMotor] = -xIn + yIn + rotation;
        wheelSpeeds[kRearRightMotor] = xIn + yIn - rotation;

        Normalize(wheelSpeeds);

        UINT8 syncGroup = 0x80;

        m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
        m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
        m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);
        m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);

        CANJaguar::UpdateSyncGroup(syncGroup);
        
        m_safetyHelper->Feed();
}

void RobotDrive::MecanumDrive_Polar(float magnitude, float direction, float rotation)
{
        // Normalized for full power along the Cartesian axes.
        magnitude = Limit(magnitude) * sqrt(2.0);
        // The rollers are at 45 degree angles.
        double dirInRad = (direction + 45.0) * 3.14159 / 180.0;
        double cosD = cos(dirInRad);
        double sinD = sin(dirInRad);

        double wheelSpeeds[kMaxNumberOfMotors];
        wheelSpeeds[kFrontLeftMotor] = sinD * magnitude + rotation;
        wheelSpeeds[kFrontRightMotor] = cosD * magnitude - rotation;
        wheelSpeeds[kRearLeftMotor] = cosD * magnitude + rotation;
        wheelSpeeds[kRearRightMotor] = sinD * magnitude - rotation;

        Normalize(wheelSpeeds);

        UINT8 syncGroup = 0x80;

        m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
        m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
        m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);
        m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);

        CANJaguar::UpdateSyncGroup(syncGroup);
        
        m_safetyHelper->Feed();
}

void RobotDrive::HolonomicDrive(float magnitude, float direction, float rotation)
{
        MecanumDrive_Polar(magnitude, direction, rotation);
}

void RobotDrive::SetLeftRightMotorOutputs(float leftOutput, float rightOutput)
{
        wpi_assert(m_rearLeftMotor != NULL && m_rearRightMotor != NULL);

        UINT8 syncGroup = 0x80;

        if (m_frontLeftMotor != NULL)
                m_frontLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
        m_rearLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);

        if (m_frontRightMotor != NULL)
                m_frontRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
        m_rearRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);

        CANJaguar::UpdateSyncGroup(syncGroup);

        m_safetyHelper->Feed();
}

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

void RobotDrive::Normalize(double *wheelSpeeds)
{
        double maxMagnitude = fabs(wheelSpeeds[0]);
        INT32 i;
        for (i=1; i<kMaxNumberOfMotors; i++)
        {
                double temp = fabs(wheelSpeeds[i]);
                if (maxMagnitude < temp) maxMagnitude = temp;
        }
        if (maxMagnitude > 1.0)
        {
                for (i=0; i<kMaxNumberOfMotors; i++)
                {
                        wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
                }
        }
}

void RobotDrive::RotateVector(double &x, double &y, double angle)
{
        double cosA = cos(angle * (3.14159 / 180.0));
        double sinA = sin(angle * (3.14159 / 180.0));
        double xOut = x * cosA - y * sinA;
        double yOut = x * sinA + y * cosA;
        x = xOut;
        y = yOut;
}

/*
 * 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_setWPIError(InvalidMotorIndex);
                return;
        }
        m_invertedMotors[motor] = isInverted ? -1 : 1;
}

void RobotDrive::SetSensitivity(float sensitivity)
{
        m_sensitivity = sensitivity;
}

void RobotDrive::SetMaxOutput(double maxOutput)
{
        m_maxOutput = maxOutput;
}



void RobotDrive::SetExpiration(float timeout)
{
        m_safetyHelper->SetExpiration(timeout);
}

float RobotDrive::GetExpiration()
{
        return m_safetyHelper->GetExpiration();
}

bool RobotDrive::IsAlive()
{
        return m_safetyHelper->IsAlive();
}

bool RobotDrive::IsSafetyEnabled()
{
        return m_safetyHelper->IsSafetyEnabled();
}

void RobotDrive::SetSafetyEnabled(bool enabled)
{
        m_safetyHelper->SetSafetyEnabled(enabled);
}

void RobotDrive::GetDescription(char *desc)
{
        sprintf(desc, "RobotDrive");
}

void RobotDrive::StopMotor()
{
        if (m_frontLeftMotor != NULL) m_frontLeftMotor->Disable();
        if (m_frontRightMotor != NULL) m_frontRightMotor->Disable();
        if (m_rearLeftMotor != NULL) m_rearLeftMotor->Disable();
        if (m_rearRightMotor != NULL) m_rearRightMotor->Disable();
}