allwpilib/wpilibc/wpilibC++Devices/src/CANTalon.cpp

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2014. 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 "CANTalon.h"
#include "WPIErrors.h"
#include "ctre/CanTalonSRX.h"
#include <unistd.h> // usleep
/**
 * Constructor for the CANTalon device.
 * @param deviceNumber The CAN ID of the Talon SRX
 */
CANTalon::CANTalon(int deviceNumber)
	: m_deviceNumber(deviceNumber)
	, m_impl(new CanTalonSRX(deviceNumber))
	, m_safetyHelper(new MotorSafetyHelper(this))
  , m_profile(0)
  , m_controlEnabled(true)
  , m_controlMode(kPercentVbus)
{
  SetControlMode(m_controlMode);
  m_impl->SetProfileSlotSelect(m_profile);
}

CANTalon::~CANTalon() {
	delete m_impl;
	delete m_safetyHelper;
}

/**
* Write out the PID value as seen in the PIDOutput base object.
*
* @deprecated Call Set instead.
*
* @param output Write out the PercentVbus value as was computed by the PIDController
*/
void CANTalon::PIDWrite(float output)
{
	if (GetControlMode() == kPercentVbus)
	{
		Set(output);
	}
	else
	{
		wpi_setWPIErrorWithContext(IncompatibleMode, "PID only supported in PercentVbus mode");
	}
}

/**
 * TODO documentation (see CANJaguar.cpp)
 */
float CANTalon::Get()
{
  int value;
  switch(m_controlMode) {
    case kVoltage:
      return GetOutputVoltage();
    case kCurrent:
      return GetOutputCurrent();
    case kSpeed:
      m_impl->GetSensorVelocity(value);
      return value;
    case kPosition:
      m_impl->GetSensorPosition(value);
      return value;
    case kPercentVbus:
    default:
      m_impl->GetAppliedThrottle(value);
      return (float)value / 1023.0;
  }
}

/**
 * Sets the appropriate output on the talon, depending on the mode.
 *
 * In PercentVbus, the output is between -1.0 and 1.0, with 0.0 as stopped.
 * In Voltage mode, outputValue is in volts.
 * In Current mode, outputValue is in amperes.
 * In Speed mode, outputValue is in position change / 10ms.
 * In Position mode, outputValue is in encoder ticks or an analog value,
 *   depending on the sensor.
 *
 * @param outputValue The setpoint value, as described above.
 * @see SelectProfileSlot to choose between the two sets of gains.
 */
void CANTalon::Set(float value, uint8_t syncGroup)
{
  if(m_controlEnabled) {
    CTR_Code status;
    switch(m_controlMode) {
      case CANSpeedController::kPercentVbus:
        {
          m_impl->Set(value);
          status = CTR_OKAY;
        }
        break;
      case CANSpeedController::kFollower:
        {
          status = m_impl->SetDemand((int)value);
        }
        break;
      case CANSpeedController::kVoltage:
        {
          // Voltage is an 8.8 fixed point number.
          int volts = int(value * 256);
          status = m_impl->SetDemand(volts);
        }
        break;
      case CANSpeedController::kSpeed:
        status = m_impl->SetDemand(value);
        break;
      case CANSpeedController::kPosition:
        status = m_impl->SetDemand(value);
        break;
      default:
        break;
    }
    if (status != CTR_OKAY) {
      wpi_setErrorWithContext(status, getHALErrorMessage(status));
    }

    status = m_impl->SetModeSelect(m_sendMode);

    if (status != CTR_OKAY) {
      wpi_setErrorWithContext(status, getHALErrorMessage(status));
    }

  }
}

/**
 * TODO documentation (see CANJaguar.cpp)
 */
void CANTalon::Disable()
{
  // Until Modes other than throttle work, just disable by setting throttle to 0.0.
  m_impl->SetModeSelect((int)CANTalon::kDisabled);
  m_controlEnabled = false;
}

/**
 * TODO documentation (see CANJaguar.cpp)
 */
void CANTalon::EnableControl() {
  SetControlMode(m_controlMode);
  m_controlEnabled = true;
}

/**
 * @param p Proportional constant to use in PID loop.
 * @see SelectProfileSlot to choose between the two sets of gains.
 */
void CANTalon::SetP(double p)
{
  CTR_Code status = m_impl->SetPgain(m_profile, p);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
}

/**
 * TODO documentation (see CANJaguar.cpp)
 * @see SelectProfileSlot to choose between the two sets of gains.
 */
void CANTalon::SetI(double i)
{
  CTR_Code status = m_impl->SetIgain(m_profile, i);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
}

/**
 * TODO documentation (see CANJaguar.cpp)
 * @see SelectProfileSlot to choose between the two sets of gains.
 */
void CANTalon::SetD(double d)
{
  CTR_Code status = m_impl->SetDgain(m_profile, d);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
}
/**
 *
 * @see SelectProfileSlot to choose between the two sets of gains.
 */
void CANTalon::SetF(double f)
{
  CTR_Code status = m_impl->SetFgain(m_profile, f);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
}
/**
 * SRX has two available slots for PID.
 * @param slotIdx one or zero depending on which slot caller wants.
 */
void CANTalon::SelectProfileSlot(int slotIdx)
{
	m_profile = (slotIdx == 0) ? 0 : 1; /* only get two slots for now */
	CTR_Code status = m_impl->SetProfileSlotSelect(m_profile);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
}
/**
 * TODO documentation (see CANJaguar.cpp)
 * This function does not modify F-gain.  Considerable passing a zero for f using
 * the four-parameter function.
 */
void CANTalon::SetPID(double p, double i, double d)
{
	SetP(p);
	SetI(i);
	SetD(d);
}
void CANTalon::SetPID(double p, double i, double d, double f)
{
	SetP(p);
	SetI(i);
	SetD(d);
	SetF(f);
}
/**
 * Select the feedback device to use in closed-loop
 */
void CANTalon::SetFeedbackDevice(FeedbackDevice device)
{
	CTR_Code status = m_impl->SetFeedbackDeviceSelect((int)device);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
}

/**
 * TODO documentation (see CANJaguar.cpp)
 * @see SelectProfileSlot to choose between the two sets of gains.
 */
double CANTalon::GetP()
{
  CanTalonSRX::param_t param = m_profile ? CanTalonSRX::eProfileParamSlot1_P : CanTalonSRX::eProfileParamSlot0_P;
  // Update the info in m_impl.
  CTR_Code status = m_impl->RequestParam(param);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
  }
  usleep(1000); /* small yield for getting response */
  double p;
  status = m_impl->GetPgain(m_profile, p);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
	return p;
}

/**
 * TODO documentation (see CANJaguar.cpp)
 * @see SelectProfileSlot to choose between the two sets of gains.
 */
double CANTalon::GetI()
{
  CanTalonSRX::param_t param = m_profile ? CanTalonSRX::eProfileParamSlot1_I : CanTalonSRX::eProfileParamSlot0_I;
  // Update the info in m_impl.
  CTR_Code status = m_impl->RequestParam(param);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
  }
	usleep(1000); /* small yield for getting response */

  double i;
  status = m_impl->GetIgain(m_profile, i);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
	return i;
}

/**
 * TODO documentation (see CANJaguar.cpp)
 * @see SelectProfileSlot to choose between the two sets of gains.
 */
double CANTalon::GetD()
{
  CanTalonSRX::param_t param = m_profile ? CanTalonSRX::eProfileParamSlot1_D : CanTalonSRX::eProfileParamSlot0_D;
  // Update the info in m_impl.
  CTR_Code status = m_impl->RequestParam(param);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
  }
	usleep(1000); /* small yield for getting response */

  double d;
  status = m_impl->GetDgain(m_profile, d);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
	return d;
}
/**
 *
 * @see SelectProfileSlot to choose between the two sets of gains.
 */
double CANTalon::GetF()
{
  CanTalonSRX::param_t param = m_profile ? CanTalonSRX::eProfileParamSlot1_F : CanTalonSRX::eProfileParamSlot0_F;
  // Update the info in m_impl.
  CTR_Code status = m_impl->RequestParam(param);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
  }

	usleep(1000);  /* small yield for getting response */
  double f;
  status = m_impl->GetFgain(m_profile, f);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
	return f;
}
/**
 * @see SelectProfileSlot to choose between the two sets of gains.
 */
double CANTalon::GetIzone()
{
  CanTalonSRX::param_t param = m_profile ? CanTalonSRX::eProfileParamSlot1_IZone: CanTalonSRX::eProfileParamSlot0_IZone;
 // Update the info in m_impl.
 CTR_Code status = m_impl->RequestParam(param);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
 }
	usleep(1000);

 int iz;
 status = m_impl->GetIzone(m_profile, iz);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
	return (double)iz;
}

/**
 * Returns the voltage coming in from the battery.
 *
 * @return The input voltage in vols.
 */
float CANTalon::GetBusVoltage()
{
  double voltage;
  CTR_Code status = m_impl->GetBatteryV(voltage);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
  return voltage;
}

/**
 * TODO documentation (see CANJaguar.cpp)
 */
float CANTalon::GetOutputVoltage()
{
  int throttle11;
  CTR_Code status = m_impl->GetAppliedThrottle(throttle11);
  float voltage = GetBusVoltage() * (float(throttle11) / 1023.0);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
	return voltage;
}


/**
 * TODO documentation (see CANJaguar.cpp)
 */
float CANTalon::GetOutputCurrent()
{
  double current;

  CTR_Code status = m_impl->GetCurrent(current);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}

	return current;
}

/**
 * TODO documentation (see CANJaguar.cpp)
 */
float CANTalon::GetTemperature()
{
  double temp;

  CTR_Code status = m_impl->GetTemp(temp);
	if(temp != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
	return temp;
}
/**
 * Set the position value of the selected sensor.  This is useful for zero-ing quadrature encoders.
 * Continuous sensors (like analog encoderes) can also partially be set (the portion of the postion based on overflows).
 */
void CANTalon::SetPosition(double pos)
{
	m_impl->SetSensorPosition(pos);
}
/**
 * TODO documentation (see CANJaguar.cpp)
 *
 * @return The position of the sensor currently providing feedback.
 * 			When using analog sensors, 0 units corresponds to 0V, 1023 units corresponds to 3.3V
 * 			When using an analog encoder (wrapping around 1023 => 0 is possible) the units are still 3.3V per 1023 units.
 * 			When using quadrature, each unit is a quadrature edge (4X) mode.
 */
double CANTalon::GetPosition()
{
  int postition;

  CTR_Code status = m_impl->GetSensorPosition(postition);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
	return (double)postition;
}
/**
 * If sensor and motor are out of phase, sensor can be inverted 
 * (position and velocity multiplied by -1).
 * @see GetPosition and @see GetSpeed.
 */
void CANTalon::SetSensorDirection(bool reverseSensor)
{
  CTR_Code status = m_impl->SetRevFeedbackSensor(reverseSensor ? 1 : 0);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
}

/**
 * Returns the current error in the controller.
 *
 * @return the difference between the setpoint and the sensor value.
 */
int CANTalon::GetClosedLoopError() {
  int error;
  CTR_Code status = m_impl->GetCloseLoopErr(error);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
  return error;
}

/**
 * TODO documentation (see CANJaguar.cpp)
 *
 * @returns The speed of the sensor currently providing feedback.
 *
 * The speed units will be in the sensor's native ticks per 100ms.
 *
 * For analog sensors, 3.3V corresponds to 1023 units.
 * 		So a speed of 200 equates to ~0.645 dV per 100ms or 6.451 dV per second.
 * 		If this is an analog encoder, that likely means 1.9548 rotations per sec.
 * For quadrature encoders, each unit corresponds a quadrature edge (4X).
 * 		So a 250 count encoder will produce 1000 edge events per rotation.
 * 		An example speed of 200 would then equate to 20% of a rotation per 100ms,
 * 		or 10 rotations per second.
 */
double CANTalon::GetSpeed()
{
  int speed;
  // TODO convert from int to appropriate units (or at least document it).

  CTR_Code status = m_impl->GetSensorVelocity(speed);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
	return (double)speed;
}

/**
 * Get the position of whatever is in the analog pin of the Talon, regardless of
 * whether it is actually being used for feedback.
 *
 * @returns The value (0 - 1023) on the analog pin of the Talon.
 */
int CANTalon::GetAnalogIn()
{
  int position;
  CTR_Code status = m_impl->GetAnalogInWithOv(position);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
  return position;
}

/**
 * Get the position of whatever is in the analog pin of the Talon, regardless of
 * whether it is actually being used for feedback.
 *
 * @returns The value (0 - 1023) on the analog pin of the Talon.
 */
int CANTalon::GetAnalogInVel()
{
  int vel;
  CTR_Code status = m_impl->GetAnalogInVel(vel);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
  return vel;
}

/**
 * Get the position of whatever is in the analog pin of the Talon, regardless of
 * whether it is actually being used for feedback.
 *
 * @returns The value (0 - 1023) on the analog pin of the Talon.
 */
int CANTalon::GetEncPosition()
{
  int position;
  CTR_Code status = m_impl->GetEncPosition(position);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
  return position;
}

/**
 * Get the position of whatever is in the analog pin of the Talon, regardless of
 * whether it is actually being used for feedback.
 *
 * @returns The value (0 - 1023) on the analog pin of the Talon.
 */
int CANTalon::GetEncVel()
{
  int vel;
  CTR_Code status = m_impl->GetEncVel(vel);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
  return vel;
}
/**
 * @return IO level of QUADA pin.
 */
int CANTalon::GetPinStateQuadA()
{
	int retval;
	CTR_Code status = m_impl->GetQuadApin(retval);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
	return retval;
}
/**
 * @return IO level of QUADB pin.
 */
int CANTalon::GetPinStateQuadB()
{
	int retval;
	CTR_Code status = m_impl->GetQuadBpin(retval);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
	return retval;
}
/**
 * @return IO level of QUAD Index pin.
 */
int CANTalon::GetPinStateQuadIdx()
{
	int retval;
	CTR_Code status = m_impl->GetQuadIdxpin(retval);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
	return retval;
}
/**
 * @return '1' iff forward limit switch is closed, 0 iff switch is open.
 * This function works regardless if limit switch feature is enabled.
 */
int CANTalon::IsFwdLimitSwitchClosed()
{
	int retval;
	CTR_Code status = m_impl->GetLimitSwitchClosedFor(retval); /* rename this func, '1' => open, '0' => closed */
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
	return retval ? 0 : 1;
}
/**
 * @return '1' iff reverse limit switch is closed, 0 iff switch is open.
 * This function works regardless if limit switch feature is enabled.
 */
int CANTalon::IsRevLimitSwitchClosed()
{
	int retval;
	CTR_Code status = m_impl->GetLimitSwitchClosedRev(retval); /* rename this func, '1' => open, '0' => closed */
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
	return retval ? 0 : 1;
}
/*
 * Simple accessor for tracked rise eventso index pin.
 * @return number of rising edges on idx pin.
 */
int CANTalon::GetNumberOfQuadIdxRises()
{
	int rises;
	CTR_Code status = m_impl->GetEncIndexRiseEvents(rises); /* rename this func, '1' => open, '0' => closed */
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
	return rises;
}
/*
 * @param rises integral value to set into index-rises register.  Great way to zero the index count.
 */
void CANTalon::SetNumberOfQuadIdxRises(int rises)
{
	CTR_Code status = m_impl->SetParam(CanTalonSRX::eEncIndexRiseEvents, rises); /* rename this func, '1' => open, '0' => closed */
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
}
/**
 * TODO documentation (see CANJaguar.cpp)
 */
bool CANTalon::GetForwardLimitOK()
{
	int limSwit=0;
	int softLim=0;
	CTR_Code status;
	status = m_impl->GetFault_ForSoftLim(softLim);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
	status = m_impl->GetFault_ForLim(limSwit);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
	/* If either fault is asserted, signal caller we are disabled (with false?) */
	return (softLim | limSwit) ? false : true;
}

/**
 * TODO documentation (see CANJaguar.cpp)
 */
bool CANTalon::GetReverseLimitOK()
{
	int limSwit=0;
	int softLim=0;
	CTR_Code status;
	status = m_impl->GetFault_RevSoftLim(softLim);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
	status = m_impl->GetFault_RevLim(limSwit);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
	/* If either fault is asserted, signal caller we are disabled (with false?) */
	return (softLim | limSwit) ? false : true;
}

/**
 * TODO documentation (see CANJaguar.cpp)
 */
uint16_t CANTalon::GetFaults()
{
	uint16_t retval = 0;
	int val;
	CTR_Code status;

	/* temperature */
	val = 0;
	status = m_impl->GetFault_OverTemp(val);
	if(status != CTR_OKAY)
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	retval |= (val) ? CANSpeedController::kTemperatureFault : 0;

	/* voltage */
	val = 0;
	status = m_impl->GetFault_UnderVoltage(val);
	if(status != CTR_OKAY)
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	retval |= (val) ? CANSpeedController::kBusVoltageFault : 0;

	/* fwd-limit-switch */
	val = 0;
	status = m_impl->GetFault_ForLim(val);
	if(status != CTR_OKAY)
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	retval |= (val) ? CANSpeedController::kFwdLimitSwitch : 0;

	/* rev-limit-switch */
	val = 0;
	status = m_impl->GetFault_RevLim(val);
	if(status != CTR_OKAY)
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	retval |= (val) ? CANSpeedController::kRevLimitSwitch : 0;

	/* fwd-soft-limit */
	val = 0;
	status = m_impl->GetFault_ForSoftLim(val);
	if(status != CTR_OKAY)
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	retval |= (val) ? CANSpeedController::kFwdSoftLimit : 0;

	/* rev-soft-limit */
	val = 0;
	status = m_impl->GetFault_RevSoftLim(val);
	if(status != CTR_OKAY)
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	retval |= (val) ? CANSpeedController::kRevSoftLimit : 0;

	return retval;
}
uint16_t CANTalon::GetStickyFaults()
{
	uint16_t retval = 0;
	int val;
	CTR_Code status;

	/* temperature */
	val = 0;
	status = m_impl->GetStckyFault_OverTemp(val);
	if(status != CTR_OKAY)
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	retval |= (val) ? CANSpeedController::kTemperatureFault : 0;

	/* voltage */
	val = 0;
	status = m_impl->GetStckyFault_UnderVoltage(val);
	if(status != CTR_OKAY)
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	retval |= (val) ? CANSpeedController::kBusVoltageFault : 0;

	/* fwd-limit-switch */
	val = 0;
	status = m_impl->GetStckyFault_ForLim(val);
	if(status != CTR_OKAY)
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	retval |= (val) ? CANSpeedController::kFwdLimitSwitch : 0;

	/* rev-limit-switch */
	val = 0;
	status = m_impl->GetStckyFault_RevLim(val);
	if(status != CTR_OKAY)
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	retval |= (val) ? CANSpeedController::kRevLimitSwitch : 0;

	/* fwd-soft-limit */
	val = 0;
	status = m_impl->GetStckyFault_ForSoftLim(val);
	if(status != CTR_OKAY)
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	retval |= (val) ? CANSpeedController::kFwdSoftLimit : 0;

	/* rev-soft-limit */
	val = 0;
	status = m_impl->GetStckyFault_RevSoftLim(val);
	if(status != CTR_OKAY)
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	retval |= (val) ? CANSpeedController::kRevSoftLimit : 0;

	return retval;
}
void CANTalon::ClearStickyFaults()
{
	CTR_Code status = m_impl->ClearStickyFaults();
	wpi_setErrorWithContext(status, getHALErrorMessage(status));
}

/**
 * Set the maximum voltage change rate.  This ramp rate is in affect regardless of which control mode
 * the TALON is in.
 *
 * When in PercentVbus or Voltage output mode, the rate at which the voltage changes can
 * be limited to reduce current spikes.  Set this to 0.0 to disable rate limiting.
 *
 * @param rampRate The maximum rate of voltage change in Percent Voltage mode in V/s.
 */
void CANTalon::SetVoltageRampRate(double rampRate)
{
	/* Caller is expressing ramp as Voltage per sec, assuming 12V is full.
		Talon's throttle ramp is in dThrot/d10ms.  1023 is full fwd, -1023 is full rev. */
	double rampRatedThrotPer10ms = (rampRate*1023.0/12.0) / 100;
	CTR_Code status = m_impl->SetRampThrottle((int)rampRatedThrotPer10ms);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
}
/**
 * Sets a voltage change rate that applies only when a close loop contorl mode is enabled.
 * This allows close loop specific ramp behavior.
 *
 * @param rampRate The maximum rate of voltage change in Percent Voltage mode in V/s.
 */
void CANTalon::SetCloseLoopRampRate(double rampRate)
{
	  CTR_Code status = m_impl->SetCloseLoopRampRate(m_profile,rampRate * 1023.0 / 12.0 / 1000.0);
		if(status != CTR_OKAY) {
			wpi_setErrorWithContext(status, getHALErrorMessage(status));
		}
}

/**
 * @return The version of the firmware running on the Talon
 */
uint32_t CANTalon::GetFirmwareVersion()
{
	int firmwareVersion;
	m_impl->RequestParam(CanTalonSRX::eFirmVers);
	usleep(1000);
	CTR_Code status = m_impl->GetParamResponseInt32(CanTalonSRX::eFirmVers,firmwareVersion);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}

	/* only sent once on boot */
	//CTR_Code status = m_impl->GetFirmVers(firmwareVersion);
	//if(status != CTR_OKAY) {
	//	wpi_setErrorWithContext(status, getHALErrorMessage(status));
	//}

	return firmwareVersion;
}

/**
 * TODO documentation (see CANJaguar.cpp)
 */
void CANTalon::ConfigNeutralMode(NeutralMode mode)
{
	CTR_Code status;
	switch(mode){
		default:
		case kNeutralMode_Jumper: /* use default setting in flash based on webdash/BrakeCal button selection */
			status = m_impl->SetOverrideBrakeType(CanTalonSRX::kBrakeOverride_UseDefaultsFromFlash);
			break;
		case kNeutralMode_Brake:
			status = m_impl->SetOverrideBrakeType(CanTalonSRX::kBrakeOverride_OverrideBrake);
			break;
		case kNeutralMode_Coast:
			status = m_impl->SetOverrideBrakeType(CanTalonSRX::kBrakeOverride_OverrideCoast);
			break;
	}
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
}

/**
 * TODO documentation (see CANJaguar.cpp)
 */
void CANTalon::ConfigEncoderCodesPerRev(uint16_t codesPerRev)
{
	/* TALON SRX does not scale units, they are raw from the sensor.  Unit scaling can be done in API or by caller */
}

/**
 * TODO documentation (see CANJaguar.cpp)
 */
void CANTalon::ConfigPotentiometerTurns(uint16_t turns)
{
	/* TALON SRX does not scale units, they are raw from the sensor.  Unit scaling can be done in API or by caller */
}

/**
 * TODO documentation (see CANJaguar.cpp)
 */
void CANTalon::ConfigSoftPositionLimits(double forwardLimitPosition, double reverseLimitPosition)
{
	ConfigLimitMode(kLimitMode_SoftPositionLimits);
	ConfigForwardLimit(forwardLimitPosition);
	ConfigReverseLimit(reverseLimitPosition);
}

/**
 * TODO documentation (see CANJaguar.cpp)
 */
void CANTalon::DisableSoftPositionLimits()
{
	ConfigLimitMode(kLimitMode_SwitchInputsOnly);
}

/**
 * TODO documentation (see CANJaguar.cpp)
 * Configures the soft limit enable (wear leveled persistent memory).
 * Also sets the limit switch overrides.
 */
void CANTalon::ConfigLimitMode(LimitMode mode)
{
	CTR_Code status;
	switch(mode){
		case kLimitMode_SwitchInputsOnly: 	/** Only use switches for limits */
			/* turn OFF both limits. SRX has individual enables and polarity for each limit switch.*/
			status = m_impl->SetForwardSoftEnable(false);
			if(status != CTR_OKAY) {
				wpi_setErrorWithContext(status, getHALErrorMessage(status));
			}
			status = m_impl->SetReverseSoftEnable(false);
			if(status != CTR_OKAY) {
				wpi_setErrorWithContext(status, getHALErrorMessage(status));
			}
			/* override enable the limit switches, this circumvents the webdash */
			status = m_impl->SetOverrideLimitSwitchEn(CanTalonSRX::kLimitSwitchOverride_EnableFwd_EnableRev);
			if(status != CTR_OKAY) {
				wpi_setErrorWithContext(status, getHALErrorMessage(status));
			}
			break;
		case kLimitMode_SoftPositionLimits: /** Use both switches and soft limits */
			/* turn on both limits. SRX has individual enables and polarity for each limit switch.*/
			status = m_impl->SetForwardSoftEnable(true);
			if(status != CTR_OKAY) {
				wpi_setErrorWithContext(status, getHALErrorMessage(status));
			}
			status = m_impl->SetReverseSoftEnable(true);
			if(status != CTR_OKAY) {
				wpi_setErrorWithContext(status, getHALErrorMessage(status));
			}
			/* override enable the limit switches, this circumvents the webdash */
			status = m_impl->SetOverrideLimitSwitchEn(CanTalonSRX::kLimitSwitchOverride_EnableFwd_EnableRev);
			if(status != CTR_OKAY) {
				wpi_setErrorWithContext(status, getHALErrorMessage(status));
			}
			break;
	}
}

/**
 * TODO documentation (see CANJaguar.cpp)
 */
void CANTalon::ConfigForwardLimit(double forwardLimitPosition)
{
	CTR_Code status;
	status = m_impl->SetForwardSoftLimit(forwardLimitPosition);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
}

/**
 * TODO documentation (see CANJaguar.cpp)
 */
void CANTalon::ConfigReverseLimit(double reverseLimitPosition)
{
	CTR_Code status;
	status = m_impl->SetReverseSoftLimit(reverseLimitPosition);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
}

/**
 * TODO documentation (see CANJaguar.cpp)
 */
void CANTalon::ConfigMaxOutputVoltage(double voltage)
{
	/* SRX does not support max output  */
	wpi_setWPIErrorWithContext(IncompatibleMode, "MaxOutputVoltage not supported.");
}

/**
 * TODO documentation (see CANJaguar.cpp)
 */
void CANTalon::ConfigFaultTime(float faultTime)
{
	/* SRX does not have fault time.  SRX motor drive is only disabled for soft limits and limit-switch faults. */
	wpi_setWPIErrorWithContext(IncompatibleMode, "Fault Time not supported.");
}

/**
 * TODO documentation (see CANJaguar.cpp)
 */
void CANTalon::SetControlMode(CANSpeedController::ControlMode mode)
{
  m_controlMode = mode;
  switch (mode) {
    case kPercentVbus:
      m_sendMode = kThrottle;
      break;
    case kCurrent:
      m_sendMode = kCurrentMode;
      break;
    case kSpeed:
      m_sendMode = kSpeedMode;
      break;
    case kPosition:
      m_sendMode = kPositionMode;
      break;
    case kVoltage:
      m_sendMode = kVoltageMode;
      break;
    case kFollower:
      m_sendMode = kFollowerMode;
      break;
  }
  // Keep the talon disabled until Set() is called.
  CTR_Code status = m_impl->SetModeSelect((int)kDisabled);
	if(status != CTR_OKAY) {
		wpi_setErrorWithContext(status, getHALErrorMessage(status));
	}
}

/**
 * TODO documentation (see CANJaguar.cpp)
 */
CANSpeedController::ControlMode CANTalon::GetControlMode()
{
  return m_controlMode;
}

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

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

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

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

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

void CANTalon::GetDescription(char *desc)
{
	sprintf(desc, "CANTalon ID %d", m_deviceNumber);
}

/**
 * Common interface for stopping the motor
 * Part of the MotorSafety interface
 *
 * @deprecated Call Disable instead.
*/
void CANTalon::StopMotor()
{
  Disable();
}