mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
d04476bb2f
Did basic testing with close loop pos with talon slaving. Change-Id: I880a29bff29a43d45b7af1be05e08b09063bf5d7
561 lines
11 KiB
C++
561 lines
11 KiB
C++
/*----------------------------------------------------------------------------*/
|
|
/* 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"
|
|
|
|
/**
|
|
* The CANTalon object is currently incomplete. As of Nov 14 2014, we only know
|
|
* for sure that sending a throttle and checking basic values (eg current,
|
|
* temperature) work.
|
|
*/
|
|
|
|
/**
|
|
* 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_controlEnabled(false)
|
|
{
|
|
// The control mode may already have been set; GetControlMode will reset
|
|
// m_controlMode to match the Talon.
|
|
GetControlMode();
|
|
}
|
|
|
|
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 kPercentVbus:
|
|
m_impl->GetAppliedThrottle(value);
|
|
return 1.0 - (float)value / 1023.0;
|
|
case kVoltage:
|
|
return GetOutputVoltage();
|
|
case kCurrent:
|
|
return GetOutputCurrent();
|
|
case kSpeed:
|
|
m_impl->GetSensorVelocity(value);
|
|
return value;
|
|
case kPosition:
|
|
m_impl->GetSensorPosition(value);
|
|
return value;
|
|
default:
|
|
break;
|
|
}
|
|
return 0.0f;
|
|
}
|
|
|
|
/**
|
|
* Sets the output set-point value.
|
|
*/
|
|
void CANTalon::Set(float value, uint8_t syncGroup)
|
|
{
|
|
if(m_controlEnabled) {
|
|
CTR_Code status;
|
|
switch(GetControlMode()) {
|
|
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:
|
|
// TODO: Add support for other modes. Need to figure out what format
|
|
// SetDemand needs.
|
|
break;
|
|
}
|
|
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->Set(0.0); // TODO when firmware is updated, remove this.
|
|
m_impl->SetModeSelect(kDisabled); // TODO when firmware is updated, uncomment this.
|
|
m_controlEnabled = false;
|
|
}
|
|
|
|
/**
|
|
* TODO documentation (see CANJaguar.cpp)
|
|
*/
|
|
void CANTalon::EnableControl() {
|
|
SetControlMode(m_controlMode);
|
|
m_controlEnabled = true;
|
|
}
|
|
|
|
/**
|
|
* @param
|
|
*/
|
|
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)
|
|
*/
|
|
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)
|
|
*/
|
|
void CANTalon::SetD(double d)
|
|
{
|
|
CTR_Code status = m_impl->SetDgain(m_profile, d);
|
|
if(status != CTR_OKAY) {
|
|
wpi_setErrorWithContext(status, getHALErrorMessage(status));
|
|
}
|
|
}
|
|
|
|
/**
|
|
* TODO documentation (see CANJaguar.cpp)
|
|
*/
|
|
void CANTalon::SetPID(double p, double i, double d)
|
|
{
|
|
SetP(p);
|
|
SetI(i);
|
|
SetD(d);
|
|
}
|
|
|
|
/**
|
|
* TODO documentation (see CANJaguar.cpp)
|
|
*/
|
|
double CANTalon::GetP()
|
|
{
|
|
double p;
|
|
CTR_Code status = m_impl->GetPgain(m_profile, p);
|
|
if(status != CTR_OKAY) {
|
|
wpi_setErrorWithContext(status, getHALErrorMessage(status));
|
|
}
|
|
return p;
|
|
}
|
|
|
|
/**
|
|
* TODO documentation (see CANJaguar.cpp)
|
|
*/
|
|
double CANTalon::GetI()
|
|
{
|
|
double i;
|
|
CTR_Code status = m_impl->GetIgain(m_profile, i);
|
|
if(status != CTR_OKAY) {
|
|
wpi_setErrorWithContext(status, getHALErrorMessage(status));
|
|
}
|
|
return i;
|
|
}
|
|
|
|
/**
|
|
* TODO documentation (see CANJaguar.cpp)
|
|
*/
|
|
double CANTalon::GetD()
|
|
{
|
|
double d;
|
|
CTR_Code status = m_impl->GetDgain(m_profile, d);
|
|
if(status != CTR_OKAY) {
|
|
wpi_setErrorWithContext(status, getHALErrorMessage(status));
|
|
}
|
|
return d;
|
|
}
|
|
|
|
/**
|
|
* 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 - 1.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;
|
|
}
|
|
|
|
/**
|
|
* TODO documentation (see CANJaguar.cpp)
|
|
*
|
|
* @return The position of the sensor currently providing feedback.
|
|
*/
|
|
double CANTalon::GetPosition()
|
|
{
|
|
int postition;
|
|
// TODO convert from int to appropriate units (or at least document it).
|
|
|
|
CTR_Code status = m_impl->GetSensorPosition(postition);
|
|
if(status != CTR_OKAY) {
|
|
wpi_setErrorWithContext(status, getHALErrorMessage(status));
|
|
}
|
|
return (double)postition;
|
|
}
|
|
|
|
/**
|
|
* TODO documentation (see CANJaguar.cpp)
|
|
*
|
|
* @returns The speed of the sensor currently providing feedback.
|
|
*/
|
|
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;
|
|
}
|
|
|
|
/**
|
|
* TODO documentation (see CANJaguar.cpp)
|
|
*/
|
|
bool CANTalon::GetForwardLimitOK()
|
|
{
|
|
// TODO
|
|
return false;
|
|
}
|
|
|
|
/**
|
|
* TODO documentation (see CANJaguar.cpp)
|
|
*/
|
|
bool CANTalon::GetReverseLimitOK()
|
|
{
|
|
// TODO
|
|
return false;
|
|
}
|
|
|
|
/**
|
|
* TODO documentation (see CANJaguar.cpp)
|
|
*/
|
|
uint16_t CANTalon::GetFaults()
|
|
{
|
|
// TODO
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* TODO documentation (see CANJaguar.cpp)
|
|
*/
|
|
void CANTalon::SetVoltageRampRate(double rampRate)
|
|
{
|
|
// TODO
|
|
}
|
|
|
|
/**
|
|
* @return The version of the firmware running on the Talon
|
|
*/
|
|
uint32_t CANTalon::GetFirmwareVersion()
|
|
{
|
|
int firmwareVersion;
|
|
|
|
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)
|
|
{
|
|
// TODO
|
|
}
|
|
|
|
/**
|
|
* TODO documentation (see CANJaguar.cpp)
|
|
*/
|
|
void CANTalon::ConfigEncoderCodesPerRev(uint16_t codesPerRev)
|
|
{
|
|
// TODO
|
|
}
|
|
|
|
/**
|
|
* TODO documentation (see CANJaguar.cpp)
|
|
*/
|
|
void CANTalon::ConfigPotentiometerTurns(uint16_t turns)
|
|
{
|
|
// TODO
|
|
}
|
|
|
|
/**
|
|
* TODO documentation (see CANJaguar.cpp)
|
|
*/
|
|
void CANTalon::ConfigSoftPositionLimits(double forwardLimitPosition, double reverseLimitPosition)
|
|
{
|
|
// TODO
|
|
}
|
|
|
|
/**
|
|
* TODO documentation (see CANJaguar.cpp)
|
|
*/
|
|
void CANTalon::DisableSoftPositionLimits()
|
|
{
|
|
// TODO
|
|
}
|
|
|
|
/**
|
|
* TODO documentation (see CANJaguar.cpp)
|
|
*/
|
|
void CANTalon::ConfigLimitMode(LimitMode mode)
|
|
{
|
|
// TODO
|
|
}
|
|
|
|
/**
|
|
* TODO documentation (see CANJaguar.cpp)
|
|
*/
|
|
void CANTalon::ConfigForwardLimit(double forwardLimitPosition)
|
|
{
|
|
// TODO
|
|
}
|
|
|
|
/**
|
|
* TODO documentation (see CANJaguar.cpp)
|
|
*/
|
|
void CANTalon::ConfigReverseLimit(double reverseLimitPosition)
|
|
{
|
|
// TODO
|
|
}
|
|
|
|
/**
|
|
* TODO documentation (see CANJaguar.cpp)
|
|
* Does this exist on the Talon?
|
|
*/
|
|
void CANTalon::ConfigMaxOutputVoltage(double voltage)
|
|
{
|
|
// TODO
|
|
}
|
|
|
|
/**
|
|
* TODO documentation (see CANJaguar.cpp)
|
|
* Does this exist on the Talon?
|
|
*/
|
|
void CANTalon::ConfigFaultTime(float faultTime)
|
|
{
|
|
// TODO
|
|
}
|
|
|
|
/**
|
|
* TODO documentation (see CANJaguar.cpp)
|
|
*/
|
|
void CANTalon::SetControlMode(CANSpeedController::ControlMode mode)
|
|
{
|
|
m_controlMode = mode;
|
|
TalonControlMode sendMode;
|
|
switch (mode) {
|
|
case kPercentVbus:
|
|
sendMode = kThrottle;
|
|
break;
|
|
case kCurrent:
|
|
sendMode = kCurrentMode;
|
|
break;
|
|
case kSpeed:
|
|
sendMode = kSpeedMode;
|
|
break;
|
|
case kPosition:
|
|
sendMode = kPositionMode;
|
|
break;
|
|
case kVoltage:
|
|
sendMode = kVoltageMode;
|
|
break;
|
|
case kFollower:
|
|
sendMode = kFollowerMode;
|
|
break;
|
|
}
|
|
CTR_Code status = m_impl->SetModeSelect((int)sendMode);
|
|
if(status != CTR_OKAY) {
|
|
wpi_setErrorWithContext(status, getHALErrorMessage(status));
|
|
}
|
|
}
|
|
|
|
/**
|
|
* TODO documentation (see CANJaguar.cpp)
|
|
*/
|
|
CANSpeedController::ControlMode CANTalon::GetControlMode()
|
|
{
|
|
// Take the opportunity to check that the control mode is what we think it is.
|
|
int temp;
|
|
CTR_Code status = m_impl->GetModeSelect(temp);
|
|
if(status != CTR_OKAY) {
|
|
wpi_setErrorWithContext(status, getHALErrorMessage(status));
|
|
}
|
|
switch ((TalonControlMode)temp) {
|
|
case kThrottle:
|
|
m_controlMode = kPercentVbus;
|
|
break;
|
|
case kCurrentMode:
|
|
m_controlMode = kCurrent;
|
|
break;
|
|
case kSpeedMode:
|
|
m_controlMode = kSpeed;
|
|
break;
|
|
case kPositionMode:
|
|
m_controlMode = kPosition;
|
|
break;
|
|
case kVoltageMode:
|
|
m_controlMode = kVoltage;
|
|
break;
|
|
case kFollowerMode:
|
|
m_controlMode = kFollower;
|
|
break;
|
|
case kDisabled:
|
|
m_controlEnabled = false;
|
|
break;
|
|
}
|
|
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();
|
|
}
|