/**
* Construct an analog input.
*
* @param channel The channel number on the roboRIO to represent. 0-3 are
* on-board 4-7 are on the MXP port.
*/
AnalogInput::AnalogInput(uint32_t channel) {
std::stringstream buf;
buf << "Analog Input " << channel;
Resource::CreateResourceObject(inputs, kAnalogInputs);
if (!checkAnalogInputChannel(channel)) {
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str());
return;
}
if (inputs->Allocate(channel, buf.str()) ==
std::numeric_limits<uint32_t>::max()) {
CloneError(*inputs);
return;
}
m_channel = channel;
void *port = getPort(channel);
int32_t status = 0;
m_port = initializeAnalogInputPort(port, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
LiveWindow::GetInstance()->AddSensor("AnalogInput", channel, this);
HALReport(HALUsageReporting::kResourceType_AnalogChannel, channel);
}
/**
* Initialize the Ultrasonic Sensor.
* This is the common code that initializes the ultrasonic sensor given that there
* are two digital I/O channels allocated. If the system was running in automatic mode (round robin)
* when the new sensor is added, it is stopped, the sensor is added, then automatic mode is
* restored.
*/
void Ultrasonic::Initialize()
{
m_table = NULL;
bool originalMode = m_automaticEnabled;
if (m_semaphore == 0) m_semaphore = initializeSemaphore(SEMAPHORE_FULL);
SetAutomaticMode(false); // kill task when adding a new sensor
takeSemaphore(m_semaphore); // link this instance on the list
{
m_nextSensor = m_firstSensor;
m_firstSensor = this;
}
giveSemaphore(m_semaphore);
m_counter = new Counter(m_echoChannel); // set up counter for this sensor
m_counter->SetMaxPeriod(1.0);
m_counter->SetSemiPeriodMode(true);
m_counter->Reset();
m_enabled = true; // make it available for round robin scheduling
SetAutomaticMode(originalMode);
static int instances = 0;
instances++;
HALReport(HALUsageReporting::kResourceType_Ultrasonic, instances);
LiveWindow::GetInstance()->AddSensor("Ultrasonic", m_echoChannel->GetChannel(), this);
}
/**
* Drive method for Mecanum wheeled robots.
*
* A method for driving with Mecanum wheeled robots. There are 4 wheels
* on the robot, arranged so that the front and back wheels are toed in 45
* degrees.
* When looking at the wheels from the top, the roller axles should form an X
* across the robot.
*
* @param magnitude The speed that the robot should drive in a given direction.
* [-1.0..1.0]
* @param direction The direction the robot should drive in degrees. The
* direction and maginitute are independent of the rotation
* rate.
* @param rotation The rate of rotation for the robot that is completely
* independent of the magnitute or direction. [-1.0..1.0]
*/
void RobotDrive::MecanumDrive_Polar(float magnitude, float direction,
float rotation) {
static bool reported = false;
if (!reported) {
HALReport(HALUsageReporting::kResourceType_RobotDrive, GetNumMotors(),
HALUsageReporting::kRobotDrive_MecanumPolar);
reported = true;
}
// 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);
m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_maxOutput);
m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_maxOutput);
m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_maxOutput);
m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_maxOutput);
m_safetyHelper->Feed();
}
/**
* Drive method for Mecanum wheeled robots.
*
* A method for driving with Mecanum wheeled robots. There are 4 wheels
* on the robot, arranged so that the front and back wheels are toed in 45
* degrees.
* When looking at the wheels from the top, the roller axles should form an X
* across the robot.
*
* This is designed to be directly driven by joystick axes.
*
* @param x The speed that the robot should drive in the X direction.
* [-1.0..1.0]
* @param y The speed that the robot should drive in the Y direction.
* This input is inverted to match the forward == -1.0 that
* joysticks produce. [-1.0..1.0]
* @param rotation The rate of rotation for the robot that is completely
* independent of the translation. [-1.0..1.0]
* @param gyroAngle The current angle reading from the gyro. Use this to
* implement field-oriented controls.
*/
void RobotDrive::MecanumDrive_Cartesian(float x, float y, float rotation,
float gyroAngle) {
static bool reported = false;
if (!reported) {
HALReport(HALUsageReporting::kResourceType_RobotDrive, GetNumMotors(),
HALUsageReporting::kRobotDrive_MecanumCartesian);
reported = true;
}
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);
m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_maxOutput);
m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_maxOutput);
m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_maxOutput);
m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_maxOutput);
m_safetyHelper->Feed();
}
/**
* Constructor.
*
* @param moduleNumber The CAN ID of the PCM the solenoid is attached to
* @param channel The channel on the PCM to control (0..7).
*/
Solenoid::Solenoid(uint8_t moduleNumber, uint32_t channel)
: SolenoidBase(moduleNumber), m_channel(channel) {
std::stringstream buf;
if (!CheckSolenoidModule(m_moduleNumber)) {
buf << "Solenoid Module " << m_moduleNumber;
wpi_setWPIErrorWithContext(ModuleIndexOutOfRange, buf.str());
return;
}
if (!CheckSolenoidChannel(m_channel)) {
buf << "Solenoid Module " << m_channel;
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str());
return;
}
Resource::CreateResourceObject(m_allocated, m_maxModules * m_maxPorts);
buf << "Solenoid " << m_channel << " (Module: " << m_moduleNumber << ")";
if (m_allocated->Allocate(m_moduleNumber * kSolenoidChannels + m_channel,
buf.str()) ==
std::numeric_limits<uint32_t>::max()) {
CloneError(*m_allocated);
return;
}
#if FULL_WPILIB
LiveWindow::GetInstance()->AddActuator("Solenoid", m_moduleNumber, m_channel,
this);
#endif
HALReport(HALUsageReporting::kResourceType_Solenoid, m_channel,
m_moduleNumber);
}
/**
* Drive the motors at "outputMagnitude" and "curve".
* Both outputMagnitude and curve are -1.0 to +1.0 values, where 0.0 represents
* stopped and not turning. curve < 0 will turn left and curve > 0 will turn
* right.
*
* The algorithm for steering provides a constant turn radius for any normal
* speed range, both forward and backward. Increasing m_sensitivity causes
* sharper turns for fixed values of curve.
*
* This function will most likely be used in an autonomous routine.
*
* @param outputMagnitude The speed setting for the outside wheel in a turn,
* forward or backwards, +1 to -1.
* @param curve The rate of turn, constant for different forward speeds. Set
* curve < 0 for left turn or curve > 0 for right turn.
* Set curve = e^(-r/w) to get a turn radius r for wheelbase w of your robot.
* Conversely, turn radius r = -ln(curve)*w for a given value of curve and
* wheelbase w.
*/
void RobotDrive::Drive(float outputMagnitude, float curve) {
float leftOutput, rightOutput;
static bool reported = false;
if (!reported) {
HALReport(HALUsageReporting::kResourceType_RobotDrive, GetNumMotors(),
HALUsageReporting::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);
}
/**
* Fixup the sendMode so Set() serializes the correct demand value.
* Also fills the modeSelecet in the control frame to disabled.
* @param mode Control mode to ultimately enter once user calls Set().
* @see Set()
*/
void CANTalon::ApplyControlMode(CANSpeedController::ControlMode mode) {
m_controlMode = mode;
HALReport(HALUsageReporting::kResourceType_CANTalonSRX, m_deviceNumber + 1,
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));
}
}
/**
* 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) {
HALReport(HALUsageReporting::kResourceType_RobotDrive, GetNumMotors(),
HALUsageReporting::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);
}
/**
* Initialize the gyro. Calibration is handled by Calibrate().
*/
void AnalogGyro::InitGyro() {
if (StatusIsFatal()) return;
if (!m_analog->IsAccumulatorChannel()) {
wpi_setWPIErrorWithContext(ParameterOutOfRange,
" channel (must be accumulator channel)");
m_analog = nullptr;
return;
}
m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond;
m_analog->SetAverageBits(kAverageBits);
m_analog->SetOversampleBits(kOversampleBits);
float sampleRate =
kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits));
m_analog->SetSampleRate(sampleRate);
Wait(0.1);
SetDeadband(0.0f);
SetPIDSourceType(PIDSourceType::kDisplacement);
HALReport(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel());
LiveWindow::GetInstance()->AddSensor("AnalogGyro", m_analog->GetChannel(), this);
}
/**
* Constructor for a VictorSP
* @param channel The PWM channel that the VictorSP is attached to. 0-9 are
* on-board, 10-19 are on the MXP port
*/
VictorSP::VictorSP(uint32_t channel) : SafePWM(channel) {
SetBounds(2.004, 1.52, 1.50, 1.48, .997);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetRaw(m_centerPwm);
SetZeroLatch();
HALReport(HALUsageReporting::kResourceType_VictorSP, GetChannel());
LiveWindow::GetInstance()->AddActuator("VictorSP", GetChannel(), this);
}
/**
* Common initialization code called by all constructors.
*
* InitServo() assigns defaults for the period multiplier for the servo PWM control signal, as
* well as the minimum and maximum PWM values supported by the servo.
*/
void Servo::InitServo()
{
m_table = NULL;
SetBounds(kDefaultMaxServoPWM, 0.0, 0.0, 0.0, kDefaultMinServoPWM);
SetPeriodMultiplier(kPeriodMultiplier_4X);
LiveWindow::GetInstance()->AddActuator("Servo", GetChannel(), this);
HALReport(HALUsageReporting::kResourceType_Servo, GetChannel());
}
/**
* Constructor for a Spark
* @param channel The PWM channel that the Spark is attached to. 0-9 are
* on-board, 10-19 are on the MXP port
*/
Spark::Spark(uint32_t channel) : PWMSpeedController(channel) {
SetBounds(2.003, 1.55, 1.50, 1.46, .999);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetRaw(m_centerPwm);
SetZeroLatch();
HALReport(HALUsageReporting::kResourceType_RevSPARK, GetChannel());
LiveWindow::GetInstance()->AddActuator("Spark", GetChannel(), this);
}
/**
* Constructor for a SD540
* @param channel The PWM channel that the SD540 is attached to. 0-9 are
* on-board, 10-19 are on the MXP port
*/
SD540::SD540(uint32_t channel) : SafePWM(channel) {
SetBounds(2.05, 1.55, 1.50, 1.44, .94);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetRaw(m_centerPwm);
SetZeroLatch();
HALReport(HALUsageReporting::kResourceType_MindsensorsSD540, GetChannel());
LiveWindow::GetInstance()->AddActuator("SD540", GetChannel(), this);
}
/**
* Common initialization code called by all constructors.
*
* Note that the Talon uses the following bounds for PWM values. These values should work reasonably well for
* most controllers, but if users experience issues such as asymmetric behavior around
* the deadband or inability to saturate the controller in either direction, calibration is recommended.
* The calibration procedure can be found in the Talon User Manual available from CTRE.
*
* 2.037ms = full "forward"
* 1.539ms = the "high end" of the deadband range
* 1.513ms = center of the deadband range (off)
* 1.487ms = the "low end" of the deadband range
* 0.989ms = full "reverse"
*/
void Talon::InitTalon() {
SetBounds(2.037, 1.539, 1.513, 1.487, .989);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetRaw(m_centerPwm);
SetZeroLatch();
HALReport(HALUsageReporting::kResourceType_Talon, GetChannel());
LiveWindow::GetInstance()->AddActuator("Talon", GetChannel(), this);
}
/**
* Constructor.
*
* @param port The I2C port the accelerometer is attached to
* @param range The range (+ or -) that the accelerometer will measure.
*/
ADXL345_I2C::ADXL345_I2C(Port port, Range range) : I2C(port, kAddress) {
// Turn on the measurements
Write(kPowerCtlRegister, kPowerCtl_Measure);
// Specify the data format to read
SetRange(range);
HALReport(HALUsageReporting::kResourceType_ADXL345,
HALUsageReporting::kADXL345_I2C, 0);
LiveWindow::GetInstance()->AddSensor("ADXL345_I2C", port, this);
}
DigitalGlitchFilter::DigitalGlitchFilter() {
std::lock_guard<priority_mutex> sync(m_mutex);
auto index =
std::find(m_filterAllocated.begin(), m_filterAllocated.end(), false);
wpi_assert(index != m_filterAllocated.end());
m_channelIndex = std::distance(m_filterAllocated.begin(), index);
*index = true;
HALReport(HALUsageReporting::kResourceType_DigitalFilter, m_channelIndex);
}
/**
* Initialize an analog trigger from a channel.
*/
void AnalogTrigger::InitTrigger(uint32_t channel)
{
void* port = getPort(channel);
int32_t status = 0;
uint32_t index = 0;
m_trigger = initializeAnalogTrigger(port, &index, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
m_index = index;
HALReport(HALUsageReporting::kResourceType_AnalogTrigger, channel);
}
Preferences::Preferences() {
std::unique_lock<priority_recursive_mutex> sync(m_fileLock);
m_readTask = Task("PreferencesReadTask", &Preferences::ReadTaskRun, this);
/* The main thread initially blocks on the semaphore. The read task signals
* the main thread to continue after it has locked the table mutex (so the
* table will be fully populated when the main thread can finally access it).
*/
m_fileOpStarted.take();
HALReport(HALUsageReporting::kResourceType_Preferences, 0);
}
/**
* Construct an instance of a joystick.
* The joystick index is the usb port on the drivers station.
*
* @param port The port on the driver station that the joystick is plugged into
* (0-5).
*/
Joystick::Joystick(uint32_t port)
: Joystick(port, kNumAxisTypes, kNumButtonTypes) {
m_axes[kXAxis] = kDefaultXAxis;
m_axes[kYAxis] = kDefaultYAxis;
m_axes[kZAxis] = kDefaultZAxis;
m_axes[kTwistAxis] = kDefaultTwistAxis;
m_axes[kThrottleAxis] = kDefaultThrottleAxis;
m_buttons[kTriggerButton] = kDefaultTriggerButton;
m_buttons[kTopButton] = kDefaultTopButton;
HALReport(HALUsageReporting::kResourceType_Joystick, port);
}
/**
* 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);
}
/**
* Relay constructor given a channel.
*
* This code initializes the relay and reserves all resources that need to be
* locked. Initially the relay is set to both lines at 0v.
* @param channel The channel number (0-3).
* @param direction The direction that the Relay object will control.
*/
Relay::Relay(uint32_t channel, Relay::Direction direction)
: m_channel(channel), m_direction(direction) {
std::stringstream buf;
Resource::CreateResourceObject(relayChannels,
dio_kNumSystems * kRelayChannels * 2);
if (!SensorBase::CheckRelayChannel(m_channel)) {
buf << "Relay Channel " << m_channel;
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str());
return;
}
if (m_direction == kBothDirections || m_direction == kForwardOnly) {
buf << "Forward Relay " << m_channel;
if (relayChannels->Allocate(m_channel * 2, buf.str()) == ~0ul) {
CloneError(*relayChannels);
return;
}
HALReport(HALUsageReporting::kResourceType_Relay, m_channel);
}
if (m_direction == kBothDirections || m_direction == kReverseOnly) {
buf << "Reverse Relay " << m_channel;
if (relayChannels->Allocate(m_channel * 2 + 1, buf.str()) == ~0ul) {
CloneError(*relayChannels);
return;
}
HALReport(HALUsageReporting::kResourceType_Relay, m_channel + 128);
}
int32_t status = 0;
setRelayForward(m_relay_ports[m_channel], false, &status);
setRelayReverse(m_relay_ports[m_channel], false, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
LiveWindow::GetInstance().AddActuator("Relay", 1, m_channel, this);
}
/**
* Common initialization code for Encoders.
* This code allocates resources for Encoders and is common to all constructors.
*
* The counter will start counting immediately.
*
* @param reverseDirection If true, counts down instead of up (this is all relative)
* @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is
* selected, then an encoder FPGA object is used and the returned counts will be 4x the encoder
* spec'd value since all rising and falling edges are counted. If 1X or 2X are selected then
* a counter object will be used and the returned value will either exactly match the spec'd count
* or be double (2x) the spec'd count.
*/
void Encoder::InitEncoder(bool reverseDirection, EncodingType encodingType)
{
m_table = NULL;
m_encodingType = encodingType;
m_index = 0;
switch (encodingType)
{
case k4X:
{
m_encodingScale = 4;
if (m_aSource->StatusIsFatal())
{
CloneError(m_aSource);
return;
}
if (m_bSource->StatusIsFatal())
{
CloneError(m_bSource);
return;
}
int32_t status = 0;
m_encoder = initializeEncoder(m_aSource->GetModuleForRouting(), m_aSource->GetChannelForRouting(),
m_aSource->GetAnalogTriggerForRouting(),
m_bSource->GetModuleForRouting(), m_bSource->GetChannelForRouting(),
m_bSource->GetAnalogTriggerForRouting(),
reverseDirection, &m_index, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
m_counter = NULL;
SetMaxPeriod(.5);
break;
}
case k1X:
case k2X:
{
m_encodingScale = encodingType == k1X ? 1 : 2;
m_counter = new Counter(m_encodingType, m_aSource, m_bSource, reverseDirection);
m_index = m_counter->GetFPGAIndex();
break;
}
default:
wpi_setErrorWithContext(-1, "Invalid encodingType argument");
break;
}
m_distancePerPulse = 1.0;
m_pidSource = kDistance;
HALReport(HALUsageReporting::kResourceType_Encoder, m_index, encodingType);
LiveWindow::GetInstance()->AddSensor("Encoder", m_aSource->GetChannelForRouting(), this);
}
void DigitalGlitchFilter::DoAdd(DigitalSource* input, int requested_index) {
// Some sources from Counters and Encoders are null. By pushing the check
// here, we catch the issue more generally.
if (input) {
int32_t status = 0;
setFilterSelect(m_digital_ports[input->GetChannelForRouting()],
requested_index, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
// Validate that we set it correctly.
int actual_index = getFilterSelect(
m_digital_ports[input->GetChannelForRouting()], &status);
wpi_assertEqual(actual_index, requested_index);
HALReport(HALUsageReporting::kResourceType_DigitalInput,
input->GetChannelForRouting());
}
}
/**
* Constructor for a Jaguar connected via PWM
* @param channel The PWM channel that the Jaguar is attached to. 0-9 are
* on-board, 10-19 are on the MXP port
*/
Jaguar::Jaguar(uint32_t channel) : PWMSpeedController(channel) {
/**
* Input profile defined by Luminary Micro.
*
* Full reverse ranges from 0.671325ms to 0.6972211ms
* Proportional reverse ranges from 0.6972211ms to 1.4482078ms
* Neutral ranges from 1.4482078ms to 1.5517922ms
* Proportional forward ranges from 1.5517922ms to 2.3027789ms
* Full forward ranges from 2.3027789ms to 2.328675ms
*/
SetBounds(2.31, 1.55, 1.507, 1.454, .697);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetRaw(m_centerPwm);
SetZeroLatch();
HALReport(HALUsageReporting::kResourceType_Jaguar, GetChannel());
LiveWindow::GetInstance()->AddActuator("Jaguar", GetChannel(), this);
}
/**
* Internal common init function.
*/
void ADXL345_SPI::Init(Range range)
{
m_spi = new SPI(m_port);
m_spi->SetClockRate(500000);
m_spi->SetMSBFirst();
m_spi->SetSampleDataOnFalling();
m_spi->SetClockActiveLow();
m_spi->SetChipSelectActiveHigh();
uint8_t commands[2];
// Turn on the measurements
commands[0] = kPowerCtlRegister;
commands[1] = kPowerCtl_Measure;
m_spi->Transaction(commands, commands, 2);
SetRange(range);
HALReport(HALUsageReporting::kResourceType_ADXL345, HALUsageReporting::kADXL345_SPI);
}
/**
* Create an instance of a DigitalInput.
* Creates a digital input given a channel. Common creation routine for all
* constructors.
*/
void DigitalInput::InitDigitalInput(uint32_t channel)
{
m_table = NULL;
char buf[64];
if (!CheckDigitalChannel(channel))
{
snprintf(buf, 64, "Digital Channel %d", channel);
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf);
return;
}
m_channel = channel;
int32_t status = 0;
allocateDIO(m_digital_ports[channel], true, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
LiveWindow::GetInstance()->AddSensor("DigitalInput", channel, this);
HALReport(HALUsageReporting::kResourceType_DigitalInput, channel);
}
/**
* Construct a TalonSRX connected via PWM
* @param channel The PWM channel that the TalonSRX is attached to. 0-9 are
* on-board, 10-19 are on the MXP port
*/
TalonSRX::TalonSRX(uint32_t channel) : SafePWM(channel) {
/* Note that the TalonSRX uses the following bounds for PWM values. These
* values should work reasonably well for most controllers, but if users
* experience issues such as asymmetric behavior around the deadband or
* inability to saturate the controller in either direction, calibration is
* recommended. The calibration procedure can be found in the TalonSRX User
* Manual available from Cross The Road Electronics.
* 2.004ms = full "forward"
* 1.52ms = the "high end" of the deadband range
* 1.50ms = center of the deadband range (off)
* 1.48ms = the "low end" of the deadband range
* 0.997ms = full "reverse"
*/
SetBounds(2.004, 1.52, 1.50, 1.48, .997);
SetPeriodMultiplier(kPeriodMultiplier_1X);
SetRaw(m_centerPwm);
SetZeroLatch();
HALReport(HALUsageReporting::kResourceType_TalonSRX, GetChannel());
LiveWindow::GetInstance()->AddActuator("TalonSRX", GetChannel(), this);
}
/**
* Gyro constructor on the specified SPI port.
*
* @param port The SPI port the gyro is attached to.
*/
ADXRS450_Gyro::ADXRS450_Gyro(SPI::Port port) : m_spi(port) {
m_spi.SetClockRate(3000000);
m_spi.SetMSBFirst();
m_spi.SetSampleDataOnRising();
m_spi.SetClockActiveHigh();
m_spi.SetChipSelectActiveLow();
// Validate the part ID
if ((ReadRegister(kPIDRegister) & 0xff00) != 0x5200) {
DriverStation::ReportError("could not find ADXRS450 gyro");
return;
}
m_spi.InitAccumulator(kSamplePeriod, 0x20000000u, 4, 0x0c000000u, 0x04000000u,
10u, 16u, true, true);
Calibrate();
HALReport(HALUsageReporting::kResourceType_ADXRS450, port);
LiveWindow::GetInstance()->AddSensor("ADXRS450_Gyro", port, this);
}
void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf,
PIDSource* source, PIDOutput* output,
float period) {
m_controlLoop = std::make_unique<Notifier>(&PIDController::Calculate, this);
m_P = Kp;
m_I = Ki;
m_D = Kd;
m_F = Kf;
m_pidInput = source;
m_pidOutput = output;
m_period = period;
m_controlLoop->StartPeriodic(m_period);
m_setpointTimer.Start();
static int32_t instances = 0;
instances++;
HALReport(HALUsageReporting::kResourceType_PIDController, instances);
}
/**
* Allocate a PWM given a channel number.
*
* Checks channel value range and allocates the appropriate channel.
* The allocation is only done to help users ensure that they don't double
* assign channels.
* @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP
* port
*/
PWM::PWM(uint32_t channel) {
std::stringstream buf;
if (!CheckPWMChannel(channel)) {
buf << "PWM Channel " << channel;
wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str());
return;
}
int32_t status = 0;
allocatePWMChannel(m_pwm_ports[channel], &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
m_channel = channel;
setPWM(m_pwm_ports[m_channel], kPwmDisabled, &status);
wpi_setErrorWithContext(status, getHALErrorMessage(status));
m_eliminateDeadband = false;
HALReport(HALUsageReporting::kResourceType_PWM, channel);
}
