SCRIPT namespace replacements

This commit is contained in:
PJ Reiniger
2025-11-07 20:00:05 -05:00
committed by Peter Johnson
parent ae6c043632
commit 9aca8e0fd6
2622 changed files with 22275 additions and 22275 deletions

View File

@@ -15,7 +15,7 @@
* Command will *not* work!
*/
class ReplaceMeCommand2
: public frc2::CommandHelper<frc2::Command, ReplaceMeCommand2> {
: public wpi::cmd::CommandHelper<wpi::cmd::Command, ReplaceMeCommand2> {
public:
/* You should consider using the more terse Command factories API instead
* https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands

View File

@@ -11,7 +11,7 @@
// For more information, see:
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
class ReplaceMeInstantCommand2
: public frc2::CommandHelper<frc2::InstantCommand,
: public wpi::cmd::CommandHelper<wpi::cmd::InstantCommand,
ReplaceMeInstantCommand2> {
public:
ReplaceMeInstantCommand2();

View File

@@ -11,7 +11,7 @@
// For more information, see:
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
class ReplaceMeParallelCommandGroup
: public frc2::CommandHelper<frc2::ParallelCommandGroup,
: public wpi::cmd::CommandHelper<wpi::cmd::ParallelCommandGroup,
ReplaceMeParallelCommandGroup> {
public:
ReplaceMeParallelCommandGroup();

View File

@@ -11,7 +11,7 @@
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
ReplaceMeParallelDeadlineGroup::ReplaceMeParallelDeadlineGroup()
// The deadline command
: CommandHelper{frc2::InstantCommand{[] {}}} {
: CommandHelper{wpi::cmd::InstantCommand{[] {}}} {
// Add your commands here, e.g.
// AddCommands(FooCommand{}, BarCommand{});
}

View File

@@ -11,7 +11,7 @@
// For more information, see:
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
class ReplaceMeParallelDeadlineGroup
: public frc2::CommandHelper<frc2::ParallelDeadlineGroup,
: public wpi::cmd::CommandHelper<wpi::cmd::ParallelDeadlineGroup,
ReplaceMeParallelDeadlineGroup> {
public:
ReplaceMeParallelDeadlineGroup();

View File

@@ -11,7 +11,7 @@
// For more information, see:
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
class ReplaceMeParallelRaceGroup
: public frc2::CommandHelper<frc2::ParallelRaceGroup,
: public wpi::cmd::CommandHelper<wpi::cmd::ParallelRaceGroup,
ReplaceMeParallelRaceGroup> {
public:
ReplaceMeParallelRaceGroup();

View File

@@ -11,7 +11,7 @@
// For more information, see:
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
class ReplaceMeSequentialCommandGroup
: public frc2::CommandHelper<frc2::SequentialCommandGroup,
: public wpi::cmd::CommandHelper<wpi::cmd::SequentialCommandGroup,
ReplaceMeSequentialCommandGroup> {
public:
ReplaceMeSequentialCommandGroup();

View File

@@ -6,7 +6,7 @@
#include <wpi/commands2/SubsystemBase.hpp>
class ReplaceMeSubsystem2 : public frc2::SubsystemBase {
class ReplaceMeSubsystem2 : public wpi::cmd::SubsystemBase {
public:
ReplaceMeSubsystem2();

View File

@@ -19,6 +19,6 @@ void Robot::RobotPeriodic() {
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -10,7 +10,7 @@
#include <wpi/hardware/led/LEDPattern.hpp>
#include <wpi/opmode/TimedRobot.hpp>
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
Robot();
void RobotPeriodic() override;
@@ -19,19 +19,19 @@ class Robot : public frc::TimedRobot {
static constexpr int kLength = 60;
// SmartIO port 1
frc::AddressableLED m_led{1};
std::array<frc::AddressableLED::LEDData, kLength>
wpi::AddressableLED m_led{1};
std::array<wpi::AddressableLED::LEDData, kLength>
m_ledBuffer; // Reuse the buffer
// Our LED strip has a density of 120 LEDs per meter
units::meter_t kLedSpacing{1 / 120.0};
wpi::units::meter_t kLedSpacing{1 / 120.0};
// Create an LED pattern that will display a rainbow across
// all hues at maximum saturation and half brightness
frc::LEDPattern m_rainbow = frc::LEDPattern::Rainbow(255, 128);
wpi::LEDPattern m_rainbow = wpi::LEDPattern::Rainbow(255, 128);
// Create a new pattern that scrolls the rainbow pattern across the LED
// strip, moving at a speed of 1 meter per second.
frc::LEDPattern m_scrollingRainbow =
wpi::LEDPattern m_scrollingRainbow =
m_rainbow.ScrollAtAbsoluteSpeed(1_mps, kLedSpacing);
};

View File

@@ -31,7 +31,7 @@
* Be aware that the performance on this is much worse than a coprocessor
* solution!
*/
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
Robot() {
// We need to run our vision program in a separate thread. If not, our robot
@@ -49,7 +49,7 @@ class Robot : public frc::TimedRobot {
private:
static void VisionThread() {
frc::AprilTagDetector detector;
wpi::apriltag::AprilTagDetector detector;
// look for tag36h11, correct 1 error bit
// hamming 1 allocates 781KB, 2 allocates 27.4 MB, 3 allocates 932 MB
// max of 1 recommended for RoboRIO 1, while hamming 2 is feasible on the
@@ -58,24 +58,24 @@ class Robot : public frc::TimedRobot {
// Set up Pose Estimator - parameters are for a Microsoft Lifecam HD-3000
// (https://www.chiefdelphi.com/t/wpilib-apriltagdetector-sample-code/421411/21)
frc::AprilTagPoseEstimator::Config poseEstConfig = {
wpi::apriltag::AprilTagPoseEstimator::Config poseEstConfig = {
.tagSize = 6.5_in,
.fx = 699.3778103158814,
.fy = 677.7161226393544,
.cx = 345.6059345433618,
.cy = 207.12741326228522};
frc::AprilTagPoseEstimator estimator(poseEstConfig);
wpi::apriltag::AprilTagPoseEstimator estimator(poseEstConfig);
// Get the USB camera from CameraServer
cs::UsbCamera camera = frc::CameraServer::StartAutomaticCapture();
wpi::cs::UsbCamera camera = wpi::CameraServer::StartAutomaticCapture();
// Set the resolution
camera.SetResolution(640, 480);
// Get a CvSink. This will capture Mats from the Camera
cs::CvSink cvSink = frc::CameraServer::GetVideo();
wpi::cs::CvSink cvSink = wpi::CameraServer::GetVideo();
// Setup a CvSource. This will send images back to the Dashboard
cs::CvSource outputStream =
frc::CameraServer::PutVideo("Detected", 640, 480);
wpi::cs::CvSource outputStream =
wpi::CameraServer::PutVideo("Detected", 640, 480);
// Mats are very memory expensive. Lets reuse this Mat.
cv::Mat mat;
@@ -88,7 +88,7 @@ class Robot : public frc::TimedRobot {
// We'll output to NT
auto tagsTable =
nt::NetworkTableInstance::GetDefault().GetTable("apriltags");
wpi::nt::NetworkTableInstance::GetDefault().GetTable("apriltags");
auto pubTags = tagsTable->GetIntegerArrayTopic("tags").Publish();
while (true) {
@@ -105,27 +105,27 @@ class Robot : public frc::TimedRobot {
cv::cvtColor(mat, grayMat, cv::COLOR_BGR2GRAY);
cv::Size g_size = grayMat.size();
frc::AprilTagDetector::Results detections =
wpi::apriltag::AprilTagDetector::Results detections =
detector.Detect(g_size.width, g_size.height, grayMat.data);
// have not seen any tags yet
tags.clear();
for (const frc::AprilTagDetection* detection : detections) {
for (const wpi::apriltag::AprilTagDetection* detection : detections) {
// remember we saw this tag
tags.push_back(detection->GetId());
// draw lines around the tag
for (int i = 0; i <= 3; i++) {
int j = (i + 1) % 4;
const frc::AprilTagDetection::Point pti = detection->GetCorner(i);
const frc::AprilTagDetection::Point ptj = detection->GetCorner(j);
const wpi::apriltag::AprilTagDetection::Point pti = detection->GetCorner(i);
const wpi::apriltag::AprilTagDetection::Point ptj = detection->GetCorner(j);
line(mat, cv::Point(pti.x, pti.y), cv::Point(ptj.x, ptj.y),
outlineColor, 2);
}
// mark the center of the tag
const frc::AprilTagDetection::Point c = detection->GetCenter();
const wpi::apriltag::AprilTagDetection::Point c = detection->GetCenter();
int ll = 10;
line(mat, cv::Point(c.x - ll, c.y), cv::Point(c.x + ll, c.y),
crossColor, 2);
@@ -138,10 +138,10 @@ class Robot : public frc::TimedRobot {
crossColor, 3);
// determine pose
frc::Transform3d pose = estimator.Estimate(*detection);
wpi::math::Transform3d pose = estimator.Estimate(*detection);
// put pose into NT
frc::Rotation3d rotation = pose.Rotation();
wpi::math::Rotation3d rotation = pose.Rotation();
tagsTable->GetEntry(fmt::format("pose_{}", detection->GetId()))
.SetDoubleArray(
{{ pose.X().value(),
@@ -164,6 +164,6 @@ class Robot : public frc::TimedRobot {
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -11,18 +11,18 @@
* This is a demo program showing the use of the DifferentialDrive class.
* Runs the motors with arcade steering.
*/
class Robot : public frc::TimedRobot {
frc::PWMSparkMax m_leftMotor{0};
frc::PWMSparkMax m_rightMotor{1};
frc::DifferentialDrive m_robotDrive{
class Robot : public wpi::TimedRobot {
wpi::PWMSparkMax m_leftMotor{0};
wpi::PWMSparkMax m_rightMotor{1};
wpi::DifferentialDrive m_robotDrive{
[&](double output) { m_leftMotor.Set(output); },
[&](double output) { m_rightMotor.Set(output); }};
frc::Joystick m_stick{0};
wpi::Joystick m_stick{0};
public:
Robot() {
wpi::SendableRegistry::AddChild(&m_robotDrive, &m_leftMotor);
wpi::SendableRegistry::AddChild(&m_robotDrive, &m_rightMotor);
wpi::util::SendableRegistry::AddChild(&m_robotDrive, &m_leftMotor);
wpi::util::SendableRegistry::AddChild(&m_robotDrive, &m_rightMotor);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
@@ -38,6 +38,6 @@ class Robot : public frc::TimedRobot {
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -11,18 +11,18 @@
* This is a demo program showing the use of the DifferentialDrive class.
* Runs the motors with split arcade steering and an Xbox controller.
*/
class Robot : public frc::TimedRobot {
frc::PWMSparkMax m_leftMotor{0};
frc::PWMSparkMax m_rightMotor{1};
frc::DifferentialDrive m_robotDrive{
class Robot : public wpi::TimedRobot {
wpi::PWMSparkMax m_leftMotor{0};
wpi::PWMSparkMax m_rightMotor{1};
wpi::DifferentialDrive m_robotDrive{
[&](double output) { m_leftMotor.Set(output); },
[&](double output) { m_rightMotor.Set(output); }};
frc::XboxController m_driverController{0};
wpi::XboxController m_driverController{0};
public:
Robot() {
wpi::SendableRegistry::AddChild(&m_robotDrive, &m_leftMotor);
wpi::SendableRegistry::AddChild(&m_robotDrive, &m_rightMotor);
wpi::util::SendableRegistry::AddChild(&m_robotDrive, &m_leftMotor);
wpi::util::SendableRegistry::AddChild(&m_robotDrive, &m_rightMotor);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
@@ -41,6 +41,6 @@ class Robot : public frc::TimedRobot {
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -29,6 +29,6 @@ void Robot::DisabledInit() {
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -13,19 +13,19 @@ Arm::Arm() {
m_encoder.SetDistancePerPulse(kArmEncoderDistPerPulse);
// Put Mechanism 2d to SmartDashboard
frc::SmartDashboard::PutData("Arm Sim", &m_mech2d);
wpi::SmartDashboard::PutData("Arm Sim", &m_mech2d);
// Set the Arm position setpoint and P constant to Preferences if the keys
// don't already exist
frc::Preferences::InitDouble(kArmPositionKey, m_armSetpoint.value());
frc::Preferences::InitDouble(kArmPKey, m_armKp);
wpi::Preferences::InitDouble(kArmPositionKey, m_armSetpoint.value());
wpi::Preferences::InitDouble(kArmPKey, m_armKp);
}
void Arm::SimulationPeriodic() {
// In this method, we update our simulation of what our arm is doing
// First, we set our "inputs" (voltages)
m_armSim.SetInput(
frc::Vectord<1>{m_motor.Get() * frc::RobotController::GetInputVoltage()});
wpi::math::Vectord<1>{m_motor.Get() * wpi::RobotController::GetInputVoltage()});
// Next, we update it. The standard loop time is 20ms.
m_armSim.Update(20_ms);
@@ -34,8 +34,8 @@ void Arm::SimulationPeriodic() {
// voltage
m_encoderSim.SetDistance(m_armSim.GetAngle().value());
// SimBattery estimates loaded battery voltages
frc::sim::RoboRioSim::SetVInVoltage(
frc::sim::BatterySim::Calculate({m_armSim.GetCurrentDraw()}));
wpi::sim::RoboRioSim::SetVInVoltage(
wpi::sim::BatterySim::Calculate({m_armSim.GetCurrentDraw()}));
// Update the Mechanism Arm angle based on the simulated arm angle
m_arm->SetAngle(m_armSim.GetAngle());
@@ -43,10 +43,10 @@ void Arm::SimulationPeriodic() {
void Arm::LoadPreferences() {
// Read Preferences for Arm setpoint and kP on entering Teleop
m_armSetpoint = units::degree_t{
frc::Preferences::GetDouble(kArmPositionKey, m_armSetpoint.value())};
if (m_armKp != frc::Preferences::GetDouble(kArmPKey, m_armKp)) {
m_armKp = frc::Preferences::GetDouble(kArmPKey, m_armKp);
m_armSetpoint = wpi::units::degree_t{
wpi::Preferences::GetDouble(kArmPositionKey, m_armSetpoint.value())};
if (m_armKp != wpi::Preferences::GetDouble(kArmPKey, m_armKp)) {
m_armKp = wpi::Preferences::GetDouble(kArmPKey, m_armKp);
m_controller.SetP(m_armKp);
}
}
@@ -55,8 +55,8 @@ void Arm::ReachSetpoint() {
// Here, we run PID control like normal, with a setpoint read from
// preferences in degrees.
double pidOutput = m_controller.Calculate(
m_encoder.GetDistance(), (units::radian_t{m_armSetpoint}.value()));
m_motor.SetVoltage(units::volt_t{pidOutput});
m_encoder.GetDistance(), (wpi::units::radian_t{m_armSetpoint}.value()));
m_motor.SetVoltage(wpi::units::volt_t{pidOutput});
}
void Arm::Stop() {

View File

@@ -31,14 +31,14 @@ inline constexpr std::string_view kArmPositionKey = "ArmPosition";
inline constexpr std::string_view kArmPKey = "ArmP";
inline constexpr double kDefaultArmKp = 50.0;
inline constexpr units::degree_t kDefaultArmSetpoint = 75.0_deg;
inline constexpr wpi::units::degree_t kDefaultArmSetpoint = 75.0_deg;
inline constexpr units::radian_t kMinAngle = -75.0_deg;
inline constexpr units::radian_t kMaxAngle = 255.0_deg;
inline constexpr wpi::units::radian_t kMinAngle = -75.0_deg;
inline constexpr wpi::units::radian_t kMaxAngle = 255.0_deg;
inline constexpr double kArmReduction = 200.0;
inline constexpr units::kilogram_t kArmMass = 8.0_kg;
inline constexpr units::meter_t kArmLength = 30.0_in;
inline constexpr wpi::units::kilogram_t kArmMass = 8.0_kg;
inline constexpr wpi::units::meter_t kArmLength = 30.0_in;
// distance per pulse = (angle per revolution) / (pulses per revolution)
// = (2 * PI rads) / (4096 pulses)

View File

@@ -12,7 +12,7 @@
/**
* This is a sample program to demonstrate the use of arm simulation.
*/
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
Robot() {}
void SimulationPeriodic() override;
@@ -21,6 +21,6 @@ class Robot : public frc::TimedRobot {
void DisabledInit() override;
private:
frc::Joystick m_joystick{kJoystickPort};
wpi::Joystick m_joystick{kJoystickPort};
Arm m_arm;
};

View File

@@ -31,38 +31,38 @@ class Arm {
private:
// The P gain for the PID controller that drives this arm.
double m_armKp = kDefaultArmKp;
units::degree_t m_armSetpoint = kDefaultArmSetpoint;
wpi::units::degree_t m_armSetpoint = kDefaultArmSetpoint;
// The arm gearbox represents a gearbox containing two Vex 775pro motors.
frc::DCMotor m_armGearbox = frc::DCMotor::Vex775Pro(2);
wpi::math::DCMotor m_armGearbox = wpi::math::DCMotor::Vex775Pro(2);
// Standard classes for controlling our arm
frc::PIDController m_controller{m_armKp, 0, 0};
frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
frc::PWMSparkMax m_motor{kMotorPort};
wpi::math::PIDController m_controller{m_armKp, 0, 0};
wpi::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
wpi::PWMSparkMax m_motor{kMotorPort};
// Simulation classes help us simulate what's going on, including gravity.
// This sim represents an arm with 2 775s, a 600:1 reduction, a mass of 5kg,
// 30in overall arm length, range of motion in [-75, 255] degrees, and noise
// with a standard deviation of 1 encoder tick.
frc::sim::SingleJointedArmSim m_armSim{
wpi::sim::SingleJointedArmSim m_armSim{
m_armGearbox,
kArmReduction,
frc::sim::SingleJointedArmSim::EstimateMOI(kArmLength, kArmMass),
wpi::sim::SingleJointedArmSim::EstimateMOI(kArmLength, kArmMass),
kArmLength,
kMinAngle,
kMaxAngle,
true,
0_deg,
{kArmEncoderDistPerPulse}};
frc::sim::EncoderSim m_encoderSim{m_encoder};
wpi::sim::EncoderSim m_encoderSim{m_encoder};
// Create a Mechanism2d display of an Arm
frc::Mechanism2d m_mech2d{60, 60};
frc::MechanismRoot2d* m_armBase = m_mech2d.GetRoot("ArmBase", 30, 30);
frc::MechanismLigament2d* m_armTower =
m_armBase->Append<frc::MechanismLigament2d>(
"Arm Tower", 30, -90_deg, 6, frc::Color8Bit{frc::Color::kBlue});
frc::MechanismLigament2d* m_arm = m_armBase->Append<frc::MechanismLigament2d>(
"Arm", 30, m_armSim.GetAngle(), 6, frc::Color8Bit{frc::Color::kYellow});
wpi::Mechanism2d m_mech2d{60, 60};
wpi::MechanismRoot2d* m_armBase = m_mech2d.GetRoot("ArmBase", 30, 30);
wpi::MechanismLigament2d* m_armTower =
m_armBase->Append<wpi::MechanismLigament2d>(
"Arm Tower", 30, -90_deg, 6, wpi::Color8Bit{wpi::Color::kBlue});
wpi::MechanismLigament2d* m_arm = m_armBase->Append<wpi::MechanismLigament2d>(
"Arm", 30, m_armSim.GetAngle(), 6, wpi::Color8Bit{wpi::Color::kYellow});
};

View File

@@ -11,11 +11,11 @@
* Distribution Panel via CAN. The information will be displayed under variables
* through the SmartDashboard.
*/
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
Robot() {
// Put the PDP itself to the dashboard
frc::SmartDashboard::PutData("PDP", &m_pdp);
wpi::SmartDashboard::PutData("PDP", &m_pdp);
}
void RobotPeriodic() override {
@@ -23,39 +23,39 @@ class Robot : public frc::TimedRobot {
// The PDP returns the current in increments of 0.125A.
// At low currents the current readings tend to be less accurate.
double current7 = m_pdp.GetCurrent(7);
frc::SmartDashboard::PutNumber("Current Channel 7", current7);
wpi::SmartDashboard::PutNumber("Current Channel 7", current7);
// Get the voltage going into the PDP, in Volts.
// The PDP returns the voltage in increments of 0.05 Volts.
double voltage = m_pdp.GetVoltage();
frc::SmartDashboard::PutNumber("Voltage", voltage);
wpi::SmartDashboard::PutNumber("Voltage", voltage);
// Retrieves the temperature of the PDP, in degrees Celsius.
double temperatureCelsius = m_pdp.GetTemperature();
frc::SmartDashboard::PutNumber("Temperature", temperatureCelsius);
wpi::SmartDashboard::PutNumber("Temperature", temperatureCelsius);
// Get the total current of all channels.
double totalCurrent = m_pdp.GetTotalCurrent();
frc::SmartDashboard::PutNumber("Total Current", totalCurrent);
wpi::SmartDashboard::PutNumber("Total Current", totalCurrent);
// Get the total power of all channels.
// Power is the bus voltage multiplied by the current with the units Watts.
double totalPower = m_pdp.GetTotalPower();
frc::SmartDashboard::PutNumber("Total Power", totalPower);
wpi::SmartDashboard::PutNumber("Total Power", totalPower);
// Get the total energy of all channels.
// Energy is the power summed over time with units Joules.
double totalEnergy = m_pdp.GetTotalEnergy();
frc::SmartDashboard::PutNumber("Total Energy", totalEnergy);
wpi::SmartDashboard::PutNumber("Total Energy", totalEnergy);
}
private:
// Object for dealing with the Power Distribution Panel (PDP).
frc::PowerDistribution m_pdp{0};
wpi::PowerDistribution m_pdp{0};
};
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -4,7 +4,7 @@
#include "Drivetrain.hpp"
void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
void Drivetrain::SetSpeeds(const wpi::math::DifferentialDriveWheelSpeeds& speeds) {
const auto leftFeedforward = m_feedforward.Calculate(speeds.left);
const auto rightFeedforward = m_feedforward.Calculate(speeds.right);
const double leftOutput = m_leftPIDController.Calculate(
@@ -12,17 +12,17 @@ void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
const double rightOutput = m_rightPIDController.Calculate(
m_rightEncoder.GetRate(), speeds.right.value());
m_leftLeader.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
m_rightLeader.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
m_leftLeader.SetVoltage(wpi::units::volt_t{leftOutput} + leftFeedforward);
m_rightLeader.SetVoltage(wpi::units::volt_t{rightOutput} + rightFeedforward);
}
void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::radians_per_second_t rot) {
void Drivetrain::Drive(wpi::units::meters_per_second_t xSpeed,
wpi::units::radians_per_second_t rot) {
SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot}));
}
void Drivetrain::UpdateOdometry() {
m_odometry.Update(m_imu.GetRotation2d(),
units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()});
wpi::units::meter_t{m_leftEncoder.GetDistance()},
wpi::units::meter_t{m_rightEncoder.GetDistance()});
}

View File

@@ -8,7 +8,7 @@
#include "Drivetrain.hpp"
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
void AutonomousPeriodic() override {
TeleopPeriodic();
@@ -32,18 +32,18 @@ class Robot : public frc::TimedRobot {
}
private:
frc::XboxController m_controller{0};
wpi::XboxController m_controller{0};
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
// to 1.
frc::SlewRateLimiter<units::scalar> m_speedLimiter{3 / 1_s};
frc::SlewRateLimiter<units::scalar> m_rotLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_speedLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_rotLimiter{3 / 1_s};
Drivetrain m_drive;
};
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -45,40 +45,40 @@ class Drivetrain {
m_rightEncoder.Reset();
}
static constexpr units::meters_per_second_t kMaxSpeed =
static constexpr wpi::units::meters_per_second_t kMaxSpeed =
3.0_mps; // 3 meters per second
static constexpr units::radians_per_second_t kMaxAngularSpeed{
static constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{
std::numbers::pi}; // 1/2 rotation per second
void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
void Drive(units::meters_per_second_t xSpeed,
units::radians_per_second_t rot);
void SetSpeeds(const wpi::math::DifferentialDriveWheelSpeeds& speeds);
void Drive(wpi::units::meters_per_second_t xSpeed,
wpi::units::radians_per_second_t rot);
void UpdateOdometry();
private:
static constexpr units::meter_t kTrackwidth = 0.381_m * 2;
static constexpr wpi::units::meter_t kTrackwidth = 0.381_m * 2;
static constexpr double kWheelRadius = 0.0508; // meters
static constexpr int kEncoderResolution = 4096;
frc::PWMSparkMax m_leftLeader{1};
frc::PWMSparkMax m_leftFollower{2};
frc::PWMSparkMax m_rightLeader{3};
frc::PWMSparkMax m_rightFollower{4};
wpi::PWMSparkMax m_leftLeader{1};
wpi::PWMSparkMax m_leftFollower{2};
wpi::PWMSparkMax m_rightLeader{3};
wpi::PWMSparkMax m_rightFollower{4};
frc::Encoder m_leftEncoder{0, 1};
frc::Encoder m_rightEncoder{2, 3};
wpi::Encoder m_leftEncoder{0, 1};
wpi::Encoder m_rightEncoder{2, 3};
frc::PIDController m_leftPIDController{1.0, 0.0, 0.0};
frc::PIDController m_rightPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController m_leftPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController m_rightPIDController{1.0, 0.0, 0.0};
frc::OnboardIMU m_imu{frc::OnboardIMU::kFlat};
wpi::OnboardIMU m_imu{wpi::OnboardIMU::kFlat};
frc::DifferentialDriveKinematics m_kinematics{kTrackwidth};
frc::DifferentialDriveOdometry m_odometry{
m_imu.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()}};
wpi::math::DifferentialDriveKinematics m_kinematics{kTrackwidth};
wpi::math::DifferentialDriveOdometry m_odometry{
m_imu.GetRotation2d(), wpi::units::meter_t{m_leftEncoder.GetDistance()},
wpi::units::meter_t{m_rightEncoder.GetDistance()}};
// Gains are for example purposes only - must be determined for your own
// robot!
frc::SimpleMotorFeedforward<units::meters> m_feedforward{1_V, 3_V / 1_mps};
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{1_V, 3_V / 1_mps};
};

View File

@@ -28,11 +28,11 @@ Drivetrain::Drivetrain() {
m_leftEncoder.Reset();
m_rightEncoder.Reset();
frc::SmartDashboard::PutData("FieldSim", &m_fieldSim);
frc::SmartDashboard::PutData("Approximation", &m_fieldApproximation);
wpi::SmartDashboard::PutData("FieldSim", &m_fieldSim);
wpi::SmartDashboard::PutData("Approximation", &m_fieldApproximation);
}
void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
void Drivetrain::SetSpeeds(const wpi::math::DifferentialDriveWheelSpeeds& speeds) {
const auto leftFeedforward = m_feedforward.Calculate(speeds.left);
const auto rightFeedforward = m_feedforward.Calculate(speeds.right);
const double leftOutput = m_leftPIDController.Calculate(
@@ -40,22 +40,22 @@ void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
const double rightOutput = m_rightPIDController.Calculate(
m_rightEncoder.GetRate(), speeds.right.value());
m_leftLeader.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
m_rightLeader.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
m_leftLeader.SetVoltage(wpi::units::volt_t{leftOutput} + leftFeedforward);
m_rightLeader.SetVoltage(wpi::units::volt_t{rightOutput} + rightFeedforward);
}
void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::radians_per_second_t rot) {
void Drivetrain::Drive(wpi::units::meters_per_second_t xSpeed,
wpi::units::radians_per_second_t rot) {
SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot}));
}
void Drivetrain::PublishCameraToObject(
frc::Pose3d objectInField, frc::Transform3d robotToCamera,
nt::DoubleArrayEntry& cameraToObjectEntry,
frc::sim::DifferentialDrivetrainSim drivetrainSimulator) {
frc::Pose3d robotInField{drivetrainSimulator.GetPose()};
frc::Pose3d cameraInField = robotInField + robotToCamera;
frc::Transform3d cameraToObject{cameraInField, objectInField};
wpi::math::Pose3d objectInField, wpi::math::Transform3d robotToCamera,
wpi::nt::DoubleArrayEntry& cameraToObjectEntry,
wpi::sim::DifferentialDrivetrainSim drivetrainSimulator) {
wpi::math::Pose3d robotInField{drivetrainSimulator.GetPose()};
wpi::math::Pose3d cameraInField = robotInField + robotToCamera;
wpi::math::Transform3d cameraToObject{cameraInField, objectInField};
// Publishes double array with Translation3D elements {x, y, z} and Rotation3D
// elements {w, x, y, z} which describe the cameraToObject transformation.
@@ -69,24 +69,24 @@ void Drivetrain::PublishCameraToObject(
cameraToObjectEntry.Set(val);
}
frc::Pose3d Drivetrain::ObjectToRobotPose(
frc::Pose3d objectInField, frc::Transform3d robotToCamera,
nt::DoubleArrayEntry& cameraToObjectEntry) {
wpi::math::Pose3d Drivetrain::ObjectToRobotPose(
wpi::math::Pose3d objectInField, wpi::math::Transform3d robotToCamera,
wpi::nt::DoubleArrayEntry& cameraToObjectEntry) {
std::vector<double> val{cameraToObjectEntry.Get()};
// Reconstruct cameraToObject Transform3D from networktables.
frc::Translation3d translation{units::meter_t{val[0]}, units::meter_t{val[1]},
units::meter_t{val[2]}};
frc::Rotation3d rotation{frc::Quaternion{val[3], val[4], val[5], val[6]}};
frc::Transform3d cameraToObject{translation, rotation};
wpi::math::Translation3d translation{wpi::units::meter_t{val[0]}, wpi::units::meter_t{val[1]},
wpi::units::meter_t{val[2]}};
wpi::math::Rotation3d rotation{wpi::math::Quaternion{val[3], val[4], val[5], val[6]}};
wpi::math::Transform3d cameraToObject{translation, rotation};
return frc::ObjectToRobotPose(objectInField, cameraToObject, robotToCamera);
return wpi::math::ObjectToRobotPose(objectInField, cameraToObject, robotToCamera);
}
void Drivetrain::UpdateOdometry() {
m_poseEstimator.Update(m_imu.GetRotation2d(),
units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()});
wpi::units::meter_t{m_leftEncoder.GetDistance()},
wpi::units::meter_t{m_rightEncoder.GetDistance()});
// Publish cameraToObject transformation to networktables --this would
// normally be handled by the computer vision solution.
@@ -95,28 +95,28 @@ void Drivetrain::UpdateOdometry() {
// Compute the robot's field-relative position exclusively from vision
// measurements.
frc::Pose3d visionMeasurement3d = ObjectToRobotPose(
wpi::math::Pose3d visionMeasurement3d = ObjectToRobotPose(
m_objectInField, m_robotToCamera, m_cameraToObjectEntryRef);
// Convert robot's pose from Pose3d to Pose2d needed to apply vision
// Convert robot's pose from wpi::math::Pose3d to wpi::math::Pose2d needed to apply vision
// measurements.
frc::Pose2d visionMeasurement2d = visionMeasurement3d.ToPose2d();
wpi::math::Pose2d visionMeasurement2d = visionMeasurement3d.ToPose2d();
// Apply vision measurements. For simulation purposes only, we don't input a
// latency delay -- on a real robot, this must be calculated based either on
// known latency or timestamps.
m_poseEstimator.AddVisionMeasurement(visionMeasurement2d,
frc::Timer::GetTimestamp());
wpi::Timer::GetTimestamp());
}
void Drivetrain::SimulationPeriodic() {
// To update our simulation, we set motor voltage inputs, update the
// simulation, and write the simulated positions and velocities to our
// simulated encoder and gyro.
m_drivetrainSimulator.SetInputs(units::volt_t{m_leftLeader.Get()} *
frc::RobotController::GetInputVoltage(),
units::volt_t{m_rightLeader.Get()} *
frc::RobotController::GetInputVoltage());
m_drivetrainSimulator.SetInputs(wpi::units::volt_t{m_leftLeader.Get()} *
wpi::RobotController::GetInputVoltage(),
wpi::units::volt_t{m_rightLeader.Get()} *
wpi::RobotController::GetInputVoltage());
m_drivetrainSimulator.Update(20_ms);
m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetLeftPosition().value());

View File

@@ -8,7 +8,7 @@
#include "Drivetrain.hpp"
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
void AutonomousPeriodic() override {
TeleopPeriodic();
@@ -36,18 +36,18 @@ class Robot : public frc::TimedRobot {
void SimulationPeriodic() override { m_drive.SimulationPeriodic(); }
private:
frc::XboxController m_controller{0};
wpi::XboxController m_controller{0};
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
// to 1.
frc::SlewRateLimiter<units::scalar> m_speedLimiter{3 / 1_s};
frc::SlewRateLimiter<units::scalar> m_rotLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_speedLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_rotLimiter{3 / 1_s};
Drivetrain m_drive;
};
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -41,9 +41,9 @@ class Drivetrain {
public:
Drivetrain();
static constexpr units::meters_per_second_t kMaxSpeed =
static constexpr wpi::units::meters_per_second_t kMaxSpeed =
3.0_mps; // 3 meters per second
static constexpr units::radians_per_second_t kMaxAngularSpeed{
static constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{
std::numbers::pi}; // 1/2 rotation per second
/**
@@ -51,15 +51,15 @@ class Drivetrain {
*
* @param speeds The desired wheel speeds.
*/
void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
void SetSpeeds(const wpi::math::DifferentialDriveWheelSpeeds& speeds);
/** Drives the robot with the given linear velocity and angular velocity.
*
* @param xSpeed Linear velocity.
* @param rot Angular Velocity.
*/
void Drive(units::meters_per_second_t xSpeed,
units::radians_per_second_t rot);
void Drive(wpi::units::meters_per_second_t xSpeed,
wpi::units::radians_per_second_t rot);
/**
* Updates the field-relative position.
@@ -90,9 +90,9 @@ class Drivetrain {
* robot's drivetrain.
*/
void PublishCameraToObject(
frc::Pose3d objectInField, frc::Transform3d robotToCamera,
nt::DoubleArrayEntry& cameraToObjectEntry,
frc::sim::DifferentialDrivetrainSim drivetrainSimulator);
wpi::math::Pose3d objectInField, wpi::math::Transform3d robotToCamera,
wpi::nt::DoubleArrayEntry& cameraToObjectEntry,
wpi::sim::DifferentialDrivetrainSim drivetrainSimulator);
/**
* Queries the camera-to-object transformation from networktables to compute
@@ -106,71 +106,71 @@ class Drivetrain {
* @param cameraToObjectEntry The networktables entry publishing and querying
* example computer vision measurements.
*/
frc::Pose3d ObjectToRobotPose(frc::Pose3d objectInField,
frc::Transform3d robotToCamera,
nt::DoubleArrayEntry& cameraToObjectEntry);
wpi::math::Pose3d ObjectToRobotPose(wpi::math::Pose3d objectInField,
wpi::math::Transform3d robotToCamera,
wpi::nt::DoubleArrayEntry& cameraToObjectEntry);
private:
static constexpr units::meter_t kTrackwidth = 0.381_m * 2;
static constexpr units::meter_t kWheelRadius = 0.0508_m;
static constexpr wpi::units::meter_t kTrackwidth = 0.381_m * 2;
static constexpr wpi::units::meter_t kWheelRadius = 0.0508_m;
static constexpr int kEncoderResolution = 4096;
static constexpr std::array<double, 7> kDefaultVal{0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0};
frc::Transform3d m_robotToCamera{
frc::Translation3d{1_m, 1_m, 1_m},
frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi / 2}}};
wpi::math::Transform3d m_robotToCamera{
wpi::math::Translation3d{1_m, 1_m, 1_m},
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi / 2}}};
nt::NetworkTableInstance m_inst{nt::NetworkTableInstance::GetDefault()};
nt::DoubleArrayTopic m_cameraToObjectTopic{
wpi::nt::NetworkTableInstance m_inst{wpi::nt::NetworkTableInstance::GetDefault()};
wpi::nt::DoubleArrayTopic m_cameraToObjectTopic{
m_inst.GetDoubleArrayTopic("m_cameraToObjectTopic")};
nt::DoubleArrayEntry m_cameraToObjectEntry =
wpi::nt::DoubleArrayEntry m_cameraToObjectEntry =
m_cameraToObjectTopic.GetEntry(kDefaultVal);
nt::DoubleArrayEntry& m_cameraToObjectEntryRef = m_cameraToObjectEntry;
wpi::nt::DoubleArrayEntry& m_cameraToObjectEntryRef = m_cameraToObjectEntry;
frc::AprilTagFieldLayout m_aprilTagFieldLayout{
frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo)};
frc::Pose3d m_objectInField{m_aprilTagFieldLayout.GetTagPose(0).value()};
wpi::apriltag::AprilTagFieldLayout m_aprilTagFieldLayout{
wpi::apriltag::AprilTagFieldLayout::LoadField(wpi::apriltag::AprilTagField::k2024Crescendo)};
wpi::math::Pose3d m_objectInField{m_aprilTagFieldLayout.GetTagPose(0).value()};
frc::PWMSparkMax m_leftLeader{1};
frc::PWMSparkMax m_leftFollower{2};
frc::PWMSparkMax m_rightLeader{3};
frc::PWMSparkMax m_rightFollower{4};
wpi::PWMSparkMax m_leftLeader{1};
wpi::PWMSparkMax m_leftFollower{2};
wpi::PWMSparkMax m_rightLeader{3};
wpi::PWMSparkMax m_rightFollower{4};
frc::Encoder m_leftEncoder{0, 1};
frc::Encoder m_rightEncoder{2, 3};
wpi::Encoder m_leftEncoder{0, 1};
wpi::Encoder m_rightEncoder{2, 3};
frc::PIDController m_leftPIDController{1.0, 0.0, 0.0};
frc::PIDController m_rightPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController m_leftPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController m_rightPIDController{1.0, 0.0, 0.0};
frc::OnboardIMU m_imu{frc::OnboardIMU::kFlat};
wpi::OnboardIMU m_imu{wpi::OnboardIMU::kFlat};
frc::DifferentialDriveKinematics m_kinematics{kTrackwidth};
wpi::math::DifferentialDriveKinematics m_kinematics{kTrackwidth};
// Gains are for example purposes only - must be determined for your own
// robot!
frc::DifferentialDrivePoseEstimator m_poseEstimator{
wpi::math::DifferentialDrivePoseEstimator m_poseEstimator{
m_kinematics,
m_imu.GetRotation2d(),
units::meter_t{m_leftEncoder.GetDistance()},
units::meter_t{m_rightEncoder.GetDistance()},
frc::Pose2d{},
wpi::units::meter_t{m_leftEncoder.GetDistance()},
wpi::units::meter_t{m_rightEncoder.GetDistance()},
wpi::math::Pose2d{},
{0.01, 0.01, 0.01},
{0.1, 0.1, 0.1}};
// Gains are for example purposes only - must be determined for your own
// robot!
frc::SimpleMotorFeedforward<units::meters> m_feedforward{1_V, 3_V / 1_mps};
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{1_V, 3_V / 1_mps};
// Simulation classes
frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder};
frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder};
frc::Field2d m_fieldSim;
frc::Field2d m_fieldApproximation;
frc::LinearSystem<2, 2, 2> m_drivetrainSystem =
frc::LinearSystemId::IdentifyDrivetrainSystem(
wpi::sim::EncoderSim m_leftEncoderSim{m_leftEncoder};
wpi::sim::EncoderSim m_rightEncoderSim{m_rightEncoder};
wpi::Field2d m_fieldSim;
wpi::Field2d m_fieldApproximation;
wpi::math::LinearSystem<2, 2, 2> m_drivetrainSystem =
wpi::math::LinearSystemId::IdentifyDrivetrainSystem(
1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_mps, 0.3_V / 1_mps_sq);
frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
m_drivetrainSystem, kTrackwidth, frc::DCMotor::CIM(2), 8, 2_in};
wpi::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
m_drivetrainSystem, kTrackwidth, wpi::math::DCMotor::CIM(2), 8, 2_in};
};

View File

@@ -13,12 +13,12 @@
*/
class ExampleGlobalMeasurementSensor {
public:
static frc::Pose2d GetEstimatedGlobalPose(
const frc::Pose2d& estimatedRobotPose) {
auto randVec = frc::MakeWhiteNoiseVector(0.1, 0.1, 0.1);
return frc::Pose2d{estimatedRobotPose.X() + units::meter_t{randVec(0)},
estimatedRobotPose.Y() + units::meter_t{randVec(1)},
static wpi::math::Pose2d GetEstimatedGlobalPose(
const wpi::math::Pose2d& estimatedRobotPose) {
auto randVec = wpi::math::MakeWhiteNoiseVector(0.1, 0.1, 0.1);
return wpi::math::Pose2d{estimatedRobotPose.X() + wpi::units::meter_t{randVec(0)},
estimatedRobotPose.Y() + wpi::units::meter_t{randVec(1)},
estimatedRobotPose.Rotation() +
frc::Rotation2d{units::radian_t{randVec(2)}}};
wpi::math::Rotation2d{wpi::units::radian_t{randVec(2)}}};
}
};

View File

@@ -8,22 +8,22 @@
void Robot::RobotPeriodic() {
// pull alliance port high if on red alliance, pull low if on blue alliance
m_allianceOutput.Set(frc::DriverStation::GetAlliance() ==
frc::DriverStation::kRed);
m_allianceOutput.Set(wpi::DriverStation::GetAlliance() ==
wpi::DriverStation::kRed);
// pull enabled port high if enabled, low if disabled
m_enabledOutput.Set(frc::DriverStation::IsEnabled());
m_enabledOutput.Set(wpi::DriverStation::IsEnabled());
// pull auto port high if in autonomous, low if in teleop (or disabled)
m_autonomousOutput.Set(frc::DriverStation::IsAutonomous());
m_autonomousOutput.Set(wpi::DriverStation::IsAutonomous());
// pull alert port high if match time remaining is between 30 and 25 seconds
auto matchTime = frc::DriverStation::GetMatchTime();
auto matchTime = wpi::DriverStation::GetMatchTime();
m_alertOutput.Set(matchTime <= 30_s && matchTime >= 25_s);
}
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -13,7 +13,7 @@
* This is a sample program demonstrating how to communicate to a light
* controller from the robot code using the roboRIO's DIO ports.
*/
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
// define ports for communication with light controller
static constexpr int kAlliancePort = 0;
@@ -24,8 +24,8 @@ class Robot : public frc::TimedRobot {
void RobotPeriodic() override;
private:
frc::DigitalOutput m_allianceOutput{kAlliancePort};
frc::DigitalOutput m_enabledOutput{kEnabledPort};
frc::DigitalOutput m_autonomousOutput{kAutonomousPort};
frc::DigitalOutput m_alertOutput{kAlertPort};
wpi::DigitalOutput m_allianceOutput{kAlliancePort};
wpi::DigitalOutput m_enabledOutput{kEnabledPort};
wpi::DigitalOutput m_autonomousOutput{kAutonomousPort};
wpi::DigitalOutput m_alertOutput{kAlertPort};
};

View File

@@ -18,7 +18,7 @@ Robot::Robot() {}
* LiveWindow and SmartDashboard integrated updating.
*/
void Robot::RobotPeriodic() {
frc2::CommandScheduler::GetInstance().Run();
wpi::cmd::CommandScheduler::GetInstance().Run();
}
/**
@@ -38,7 +38,7 @@ void Robot::AutonomousInit() {
m_autonomousCommand = m_container.GetAutonomousCommand();
if (m_autonomousCommand) {
frc2::CommandScheduler::GetInstance().Schedule(m_autonomousCommand.value());
wpi::cmd::CommandScheduler::GetInstance().Schedule(m_autonomousCommand.value());
}
}
@@ -67,6 +67,6 @@ void Robot::TestPeriodic() {}
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -13,7 +13,7 @@ RobotContainer::RobotContainer() {
ConfigureButtonBindings();
// Set up default drive command
m_drive.SetDefaultCommand(frc2::cmd::Run(
m_drive.SetDefaultCommand(wpi::cmd::cmd::Run(
[this] {
m_drive.ArcadeDrive(-m_driverController.GetLeftY(),
-m_driverController.GetRightX());
@@ -40,7 +40,7 @@ void RobotContainer::ConfigureButtonBindings() {
m_drive.DynamicProfiledDriveDistance(3_m).WithTimeout(10_s));
}
frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
wpi::cmd::CommandPtr RobotContainer::GetAutonomousCommand() {
// Runs the chosen command in autonomous
return frc2::cmd::None();
return wpi::cmd::cmd::None();
}

View File

@@ -14,8 +14,8 @@ DriveSubsystem::DriveSubsystem()
m_rightLeader{kRightMotor1Port},
m_rightFollower{kRightMotor2Port},
m_feedforward{ks, kv, ka} {
wpi::SendableRegistry::AddChild(&m_drive, &m_leftLeader);
wpi::SendableRegistry::AddChild(&m_drive, &m_rightLeader);
wpi::util::SendableRegistry::AddChild(&m_drive, &m_leftLeader);
wpi::util::SendableRegistry::AddChild(&m_drive, &m_rightLeader);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
@@ -34,21 +34,21 @@ void DriveSubsystem::Periodic() {
}
void DriveSubsystem::SetDriveStates(
frc::TrapezoidProfile<units::meters>::State currentLeft,
frc::TrapezoidProfile<units::meters>::State currentRight,
frc::TrapezoidProfile<units::meters>::State nextLeft,
frc::TrapezoidProfile<units::meters>::State nextRight) {
wpi::math::TrapezoidProfile<wpi::units::meters>::State currentLeft,
wpi::math::TrapezoidProfile<wpi::units::meters>::State currentRight,
wpi::math::TrapezoidProfile<wpi::units::meters>::State nextLeft,
wpi::math::TrapezoidProfile<wpi::units::meters>::State nextRight) {
// Feedforward is divided by battery voltage to normalize it to [-1, 1]
m_leftLeader.SetSetpoint(
ExampleSmartMotorController::PIDMode::kPosition,
currentLeft.position.value(),
m_feedforward.Calculate(currentLeft.velocity, nextLeft.velocity) /
frc::RobotController::GetBatteryVoltage());
wpi::RobotController::GetBatteryVoltage());
m_rightLeader.SetSetpoint(
ExampleSmartMotorController::PIDMode::kPosition,
currentRight.position.value(),
m_feedforward.Calculate(currentRight.velocity, nextRight.velocity) /
frc::RobotController::GetBatteryVoltage());
wpi::RobotController::GetBatteryVoltage());
}
void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
@@ -60,20 +60,20 @@ void DriveSubsystem::ResetEncoders() {
m_rightLeader.ResetEncoder();
}
units::meter_t DriveSubsystem::GetLeftEncoderDistance() {
return units::meter_t{m_leftLeader.GetEncoderDistance()};
wpi::units::meter_t DriveSubsystem::GetLeftEncoderDistance() {
return wpi::units::meter_t{m_leftLeader.GetEncoderDistance()};
}
units::meter_t DriveSubsystem::GetRightEncoderDistance() {
return units::meter_t{m_rightLeader.GetEncoderDistance()};
wpi::units::meter_t DriveSubsystem::GetRightEncoderDistance() {
return wpi::units::meter_t{m_rightLeader.GetEncoderDistance()};
}
void DriveSubsystem::SetMaxOutput(double maxOutput) {
m_drive.SetMaxOutput(maxOutput);
}
frc2::CommandPtr DriveSubsystem::ProfiledDriveDistance(
units::meter_t distance) {
wpi::cmd::CommandPtr DriveSubsystem::ProfiledDriveDistance(
wpi::units::meter_t distance) {
return StartRun(
[&] {
// Restart timer so profile setpoints start at the beginning
@@ -94,8 +94,8 @@ frc2::CommandPtr DriveSubsystem::ProfiledDriveDistance(
.Until([&] { return m_profile.IsFinished(0_s); });
}
frc2::CommandPtr DriveSubsystem::DynamicProfiledDriveDistance(
units::meter_t distance) {
wpi::cmd::CommandPtr DriveSubsystem::DynamicProfiledDriveDistance(
wpi::units::meter_t distance) {
return StartRun(
[&] {
// Restart timer so profile setpoints start at the beginning

View File

@@ -22,7 +22,7 @@
*/
namespace DriveConstants {
inline constexpr units::second_t kDt{0.02};
inline constexpr wpi::units::second_t kDt{0.02};
inline constexpr int kLeftMotor1Port = 0;
inline constexpr int kLeftMotor2Port = 1;
inline constexpr int kRightMotor1Port = 2;

View File

@@ -11,7 +11,7 @@
#include "RobotContainer.hpp"
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
Robot();
void RobotPeriodic() override;
@@ -26,7 +26,7 @@ class Robot : public frc::TimedRobot {
private:
// Have it null by default so that if testing teleop it
// doesn't have undefined behavior and potentially crash.
std::optional<frc2::CommandPtr> m_autonomousCommand;
std::optional<wpi::cmd::CommandPtr> m_autonomousCommand;
RobotContainer m_container;
};

View File

@@ -23,11 +23,11 @@ class RobotContainer {
public:
RobotContainer();
frc2::CommandPtr GetAutonomousCommand();
wpi::cmd::CommandPtr GetAutonomousCommand();
private:
// The driver's controller
frc2::CommandXboxController m_driverController{
wpi::cmd::CommandXboxController m_driverController{
OIConstants::kDriverControllerPort};
// The robot's subsystems and commands are defined here...
@@ -36,10 +36,10 @@ class RobotContainer {
DriveSubsystem m_drive;
// RobotContainer-owned commands
frc2::CommandPtr m_driveHalfSpeed =
frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(0.5); }, {});
frc2::CommandPtr m_driveFullSpeed =
frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(1); }, {});
wpi::cmd::CommandPtr m_driveHalfSpeed =
wpi::cmd::cmd::RunOnce([this] { m_drive.SetMaxOutput(0.5); }, {});
wpi::cmd::CommandPtr m_driveFullSpeed =
wpi::cmd::cmd::RunOnce([this] { m_drive.SetMaxOutput(1); }, {});
void ConfigureButtonBindings();
};

View File

@@ -16,7 +16,7 @@
#include "Constants.hpp"
#include "ExampleSmartMotorController.hpp"
class DriveSubsystem : public frc2::SubsystemBase {
class DriveSubsystem : public wpi::cmd::SubsystemBase {
public:
DriveSubsystem();
@@ -35,10 +35,10 @@ class DriveSubsystem : public frc2::SubsystemBase {
* @param nextLeft The next left wheel state.
* @param nextRight The next right wheel state.
*/
void SetDriveStates(frc::TrapezoidProfile<units::meters>::State currentLeft,
frc::TrapezoidProfile<units::meters>::State currentRight,
frc::TrapezoidProfile<units::meters>::State nextLeft,
frc::TrapezoidProfile<units::meters>::State nextRight);
void SetDriveStates(wpi::math::TrapezoidProfile<wpi::units::meters>::State currentLeft,
wpi::math::TrapezoidProfile<wpi::units::meters>::State currentRight,
wpi::math::TrapezoidProfile<wpi::units::meters>::State nextLeft,
wpi::math::TrapezoidProfile<wpi::units::meters>::State nextRight);
/**
* Drives the robot using arcade controls.
@@ -58,14 +58,14 @@ class DriveSubsystem : public frc2::SubsystemBase {
*
* @return the average of the TWO encoder readings
*/
units::meter_t GetLeftEncoderDistance();
wpi::units::meter_t GetLeftEncoderDistance();
/**
* Gets the distance of the right encoder.
*
* @return the average of the TWO encoder readings
*/
units::meter_t GetRightEncoderDistance();
wpi::units::meter_t GetRightEncoderDistance();
/**
* Sets the max output of the drive. Useful for scaling the drive to drive
@@ -82,7 +82,7 @@ class DriveSubsystem : public frc2::SubsystemBase {
* @param distance The distance to drive forward.
* @return A command.
*/
frc2::CommandPtr ProfiledDriveDistance(units::meter_t distance);
wpi::cmd::CommandPtr ProfiledDriveDistance(wpi::units::meter_t distance);
/**
* Creates a command to drive forward a specified distance using a motion
@@ -91,14 +91,14 @@ class DriveSubsystem : public frc2::SubsystemBase {
* @param distance The distance to drive forward.
* @return A command.
*/
frc2::CommandPtr DynamicProfiledDriveDistance(units::meter_t distance);
wpi::cmd::CommandPtr DynamicProfiledDriveDistance(wpi::units::meter_t distance);
private:
frc::TrapezoidProfile<units::meters> m_profile{
wpi::math::TrapezoidProfile<wpi::units::meters> m_profile{
{DriveConstants::kMaxSpeed, DriveConstants::kMaxAcceleration}};
frc::Timer m_timer;
units::meter_t m_initialLeftDistance;
units::meter_t m_initialRightDistance;
wpi::Timer m_timer;
wpi::units::meter_t m_initialLeftDistance;
wpi::units::meter_t m_initialRightDistance;
// Components (e.g. motor controllers and sensors) should generally be
// declared private and exposed only through public methods.
@@ -109,10 +109,10 @@ class DriveSubsystem : public frc2::SubsystemBase {
ExampleSmartMotorController m_rightFollower;
// A feedforward component for the drive
frc::SimpleMotorFeedforward<units::meters> m_feedforward;
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward;
// The robot's drive
frc::DifferentialDrive m_drive{
wpi::DifferentialDrive m_drive{
[&](double output) { m_leftLeader.Set(output); },
[&](double output) { m_rightLeader.Set(output); }};
};

View File

@@ -10,14 +10,14 @@
constexpr double fullRange = 1.3;
constexpr double expectedZero = 0.0;
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
// 2nd parameter is the range of values. This sensor will output between
// 0 and the passed in value.
// 3rd parameter is the the physical value where you want "0" to be. How
// to measure this is fairly easy. Set the value to 0, place the mechanism
// where you want "0" to be, and observe the value on the dashboard, That
// is the value to enter for the 3rd parameter.
frc::DutyCycleEncoder m_dutyCycleEncoder{0, fullRange, expectedZero};
wpi::DutyCycleEncoder m_dutyCycleEncoder{0, fullRange, expectedZero};
public:
Robot() {
@@ -56,18 +56,18 @@ class Robot : public frc::TimedRobot {
// This does not change where "0" is, so no calibration numbers need
// to be changed.
double percentOfRange = fullRange * 0.1;
double shiftedOutput = frc::InputModulus(output, 0 - percentOfRange,
double shiftedOutput = wpi::math::InputModulus(output, 0 - percentOfRange,
fullRange - percentOfRange);
frc::SmartDashboard::PutBoolean("Connected", connected);
frc::SmartDashboard::PutNumber("Frequency", frequency.value());
frc::SmartDashboard::PutNumber("Output", output);
frc::SmartDashboard::PutNumber("ShiftedOutput", shiftedOutput);
wpi::SmartDashboard::PutBoolean("Connected", connected);
wpi::SmartDashboard::PutNumber("Frequency", frequency.value());
wpi::SmartDashboard::PutNumber("Output", output);
wpi::SmartDashboard::PutNumber("ShiftedOutput", shiftedOutput);
}
};
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -7,8 +7,8 @@
#include <wpi/opmode/TimedRobot.hpp>
#include <wpi/smartdashboard/SmartDashboard.hpp>
class Robot : public frc::TimedRobot {
frc::DutyCycle m_dutyCycle{0}; // Duty cycle input
class Robot : public wpi::TimedRobot {
wpi::DutyCycle m_dutyCycle{0}; // Duty cycle input
public:
Robot() {}
@@ -21,13 +21,13 @@ class Robot : public frc::TimedRobot {
// 1 is fully on, 0 is fully off
auto output = m_dutyCycle.GetOutput();
frc::SmartDashboard::PutNumber("Frequency", frequency.value());
frc::SmartDashboard::PutNumber("Duty Cycle", output);
wpi::SmartDashboard::PutNumber("Frequency", frequency.value());
wpi::SmartDashboard::PutNumber("Duty Cycle", output);
}
};
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -16,9 +16,9 @@
#include "ExampleSmartMotorController.hpp"
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
static constexpr units::second_t kDt = 20_ms;
static constexpr wpi::units::second_t kDt = 20_ms;
Robot() {
// Note: These gains are fake, and will have to be tuned for your robot.
@@ -46,22 +46,22 @@ class Robot : public frc::TimedRobot {
}
private:
frc::Joystick m_joystick{1};
wpi::Joystick m_joystick{1};
ExampleSmartMotorController m_motor{1};
frc::SimpleMotorFeedforward<units::meters> m_feedforward{
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{
// Note: These gains are fake, and will have to be tuned for your robot.
1_V, 1_V / 1_mps, 1_V / 1_mps_sq};
// Create a motion profile with the given maximum velocity and maximum
// acceleration constraints for the next setpoint.
frc::ExponentialProfile<units::meters, units::volts> m_profile{
wpi::math::ExponentialProfile<wpi::units::meters, wpi::units::volts> m_profile{
{10_V, 1_V / 1_mps, 1_V / 1_mps_sq}};
frc::ExponentialProfile<units::meters, units::volts>::State m_goal;
frc::ExponentialProfile<units::meters, units::volts>::State m_setpoint;
wpi::math::ExponentialProfile<wpi::units::meters, wpi::units::volts>::State m_goal;
wpi::math::ExponentialProfile<wpi::units::meters, wpi::units::volts>::State m_setpoint;
};
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -39,6 +39,6 @@ void Robot::DisabledInit() {
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -14,14 +14,14 @@ Elevator::Elevator() {
// Put Mechanism 2d to SmartDashboard
// To view the Elevator visualization, select Network Tables -> SmartDashboard
// -> Elevator Sim
frc::SmartDashboard::PutData("Elevator Sim", &m_mech2d);
wpi::SmartDashboard::PutData("Elevator Sim", &m_mech2d);
}
void Elevator::SimulationPeriodic() {
// In this method, we update our simulation of what our elevator is doing
// First, we set our "inputs" (voltages)
m_elevatorSim.SetInput(frc::Vectord<1>{
m_motorSim.GetSpeed() * frc::RobotController::GetInputVoltage()});
m_elevatorSim.SetInput(wpi::math::Vectord<1>{
m_motorSim.GetSpeed() * wpi::RobotController::GetInputVoltage()});
// Next, we update it. The standard loop time is 20ms.
m_elevatorSim.Update(20_ms);
@@ -30,8 +30,8 @@ void Elevator::SimulationPeriodic() {
// voltage
m_encoderSim.SetDistance(m_elevatorSim.GetPosition().value());
// SimBattery estimates loaded battery voltages
frc::sim::RoboRioSim::SetVInVoltage(
frc::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()}));
wpi::sim::RoboRioSim::SetVInVoltage(
wpi::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()}));
}
void Elevator::UpdateTelemetry() {
@@ -39,8 +39,8 @@ void Elevator::UpdateTelemetry() {
m_elevatorMech2d->SetLength(m_encoder.GetDistance());
}
void Elevator::ReachGoal(units::meter_t goal) {
frc::ExponentialProfile<units::meters, units::volts>::State goalState{goal,
void Elevator::ReachGoal(wpi::units::meter_t goal) {
wpi::math::ExponentialProfile<wpi::units::meters, wpi::units::volts>::State goalState{goal,
0_mps};
auto next = m_profile.Calculate(20_ms, m_setpoint, goalState);
@@ -50,7 +50,7 @@ void Elevator::ReachGoal(units::meter_t goal) {
auto feedforwardOutput =
m_feedforward.Calculate(m_setpoint.velocity, next.velocity);
m_motor.SetVoltage(units::volt_t{pidOutput} + feedforwardOutput);
m_motor.SetVoltage(wpi::units::volt_t{pidOutput} + feedforwardOutput);
m_setpoint = next;
}

View File

@@ -34,20 +34,20 @@ inline constexpr double kElevatorKp = 0.75;
inline constexpr double kElevatorKi = 0.0;
inline constexpr double kElevatorKd = 0.0;
inline constexpr units::volt_t kElevatorMaxV = 10_V;
inline constexpr units::volt_t kElevatorkS = 0.0_V;
inline constexpr units::volt_t kElevatorkG = 0.62_V;
inline constexpr wpi::units::volt_t kElevatorMaxV = 10_V;
inline constexpr wpi::units::volt_t kElevatorkS = 0.0_V;
inline constexpr wpi::units::volt_t kElevatorkG = 0.62_V;
inline constexpr auto kElevatorkV = 3.9_V / 1_mps;
inline constexpr auto kElevatorkA = 0.06_V / 1_mps_sq;
inline constexpr double kElevatorGearing = 5.0;
inline constexpr units::meter_t kElevatorDrumRadius = 1_in;
inline constexpr units::kilogram_t kCarriageMass = 12_lb;
inline constexpr wpi::units::meter_t kElevatorDrumRadius = 1_in;
inline constexpr wpi::units::kilogram_t kCarriageMass = 12_lb;
inline constexpr units::meter_t kSetpoint = 42.875_in;
inline constexpr units::meter_t kLowerSetpoint = 15_in;
inline constexpr units::meter_t kMinElevatorHeight = 0_cm;
inline constexpr units::meter_t kMaxElevatorHeight = 50_in;
inline constexpr wpi::units::meter_t kSetpoint = 42.875_in;
inline constexpr wpi::units::meter_t kLowerSetpoint = 15_in;
inline constexpr wpi::units::meter_t kMinElevatorHeight = 0_cm;
inline constexpr wpi::units::meter_t kMaxElevatorHeight = 50_in;
// distance per pulse = (distance per revolution) / (pulses per revolution)
// = (Pi * D) / ppr

View File

@@ -12,7 +12,7 @@
/**
* This is a sample program to demonstrate the use of elevator simulation.
*/
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
Robot() {}
void RobotPeriodic() override;
@@ -22,6 +22,6 @@ class Robot : public frc::TimedRobot {
void DisabledInit() override;
private:
frc::Joystick m_joystick{Constants::kJoystickPort};
wpi::Joystick m_joystick{Constants::kJoystickPort};
Elevator m_elevator;
};

View File

@@ -26,34 +26,34 @@ class Elevator {
Elevator();
void SimulationPeriodic();
void UpdateTelemetry();
void ReachGoal(units::meter_t goal);
void ReachGoal(wpi::units::meter_t goal);
void Reset();
void Stop();
private:
// This gearbox represents a gearbox containing 4 Vex 775pro motors.
frc::DCMotor m_elevatorGearbox = frc::DCMotor::NEO(2);
wpi::math::DCMotor m_elevatorGearbox = wpi::math::DCMotor::NEO(2);
// Standard classes for controlling our elevator
frc::ExponentialProfile<units::meters, units::volts>::Constraints
wpi::math::ExponentialProfile<wpi::units::meters, wpi::units::volts>::Constraints
m_constraints{Constants::kElevatorMaxV, Constants::kElevatorkV,
Constants::kElevatorkA};
frc::ExponentialProfile<units::meters, units::volts> m_profile{m_constraints};
frc::ExponentialProfile<units::meters, units::volts>::State m_setpoint;
wpi::math::ExponentialProfile<wpi::units::meters, wpi::units::volts> m_profile{m_constraints};
wpi::math::ExponentialProfile<wpi::units::meters, wpi::units::volts>::State m_setpoint;
frc::PIDController m_controller{
wpi::math::PIDController m_controller{
Constants::kElevatorKp, Constants::kElevatorKi, Constants::kElevatorKd};
frc::ElevatorFeedforward m_feedforward{
wpi::math::ElevatorFeedforward m_feedforward{
Constants::kElevatorkS, Constants::kElevatorkG, Constants::kElevatorkV,
Constants::kElevatorkA};
frc::Encoder m_encoder{Constants::kEncoderAChannel,
wpi::Encoder m_encoder{Constants::kEncoderAChannel,
Constants::kEncoderBChannel};
frc::PWMSparkMax m_motor{Constants::kMotorPort};
frc::sim::PWMMotorControllerSim m_motorSim{m_motor};
wpi::PWMSparkMax m_motor{Constants::kMotorPort};
wpi::sim::PWMMotorControllerSim m_motorSim{m_motor};
// Simulation classes help us simulate what's going on, including gravity.
frc::sim::ElevatorSim m_elevatorSim{m_elevatorGearbox,
wpi::sim::ElevatorSim m_elevatorSim{m_elevatorGearbox,
Constants::kElevatorGearing,
Constants::kCarriageMass,
Constants::kElevatorDrumRadius,
@@ -62,13 +62,13 @@ class Elevator {
true,
0_m,
{0.005}};
frc::sim::EncoderSim m_encoderSim{m_encoder};
wpi::sim::EncoderSim m_encoderSim{m_encoder};
// Create a Mechanism2d display of an elevator
frc::Mechanism2d m_mech2d{10_in / 1_m, 51_in / 1_m};
frc::MechanismRoot2d* m_elevatorRoot =
wpi::Mechanism2d m_mech2d{10_in / 1_m, 51_in / 1_m};
wpi::MechanismRoot2d* m_elevatorRoot =
m_mech2d.GetRoot("Elevator Root", 5_in / 1_m, 0.5_in / 1_m);
frc::MechanismLigament2d* m_elevatorMech2d =
m_elevatorRoot->Append<frc::MechanismLigament2d>(
wpi::MechanismLigament2d* m_elevatorMech2d =
m_elevatorRoot->Append<wpi::MechanismLigament2d>(
"Elevator", m_elevatorSim.GetPosition().value(), 90_deg);
};

View File

@@ -17,9 +17,9 @@
#include <wpi/units/velocity.hpp>
#include <wpi/units/voltage.hpp>
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
static constexpr units::second_t kDt = 20_ms;
static constexpr wpi::units::second_t kDt = 20_ms;
Robot() {
m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * std::numbers::pi * 1.5);
@@ -34,37 +34,37 @@ class Robot : public frc::TimedRobot {
// Run controller and update motor output
m_motor.SetVoltage(
units::volt_t{
m_controller.Calculate(units::meter_t{m_encoder.GetDistance()})} +
wpi::units::volt_t{
m_controller.Calculate(wpi::units::meter_t{m_encoder.GetDistance()})} +
m_feedforward.Calculate(m_controller.GetSetpoint().velocity));
}
private:
static constexpr units::meters_per_second_t kMaxVelocity = 1.75_mps;
static constexpr units::meters_per_second_squared_t kMaxAcceleration =
static constexpr wpi::units::meters_per_second_t kMaxVelocity = 1.75_mps;
static constexpr wpi::units::meters_per_second_squared_t kMaxAcceleration =
0.75_mps_sq;
static constexpr double kP = 1.3;
static constexpr double kI = 0.0;
static constexpr double kD = 0.7;
static constexpr units::volt_t kS = 1.1_V;
static constexpr units::volt_t kG = 1.2_V;
static constexpr wpi::units::volt_t kS = 1.1_V;
static constexpr wpi::units::volt_t kG = 1.2_V;
static constexpr auto kV = 1.3_V / 1_mps;
frc::Joystick m_joystick{1};
frc::Encoder m_encoder{1, 2};
frc::PWMSparkMax m_motor{1};
wpi::Joystick m_joystick{1};
wpi::Encoder m_encoder{1, 2};
wpi::PWMSparkMax m_motor{1};
// Create a PID controller whose setpoint's change is subject to maximum
// velocity and acceleration constraints.
frc::TrapezoidProfile<units::meters>::Constraints m_constraints{
wpi::math::TrapezoidProfile<wpi::units::meters>::Constraints m_constraints{
kMaxVelocity, kMaxAcceleration};
frc::ProfiledPIDController<units::meters> m_controller{kP, kI, kD,
wpi::math::ProfiledPIDController<wpi::units::meters> m_controller{kP, kI, kD,
m_constraints, kDt};
frc::ElevatorFeedforward m_feedforward{kS, kG, kV};
wpi::math::ElevatorFeedforward m_feedforward{kS, kG, kV};
};
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -34,6 +34,6 @@ void Robot::DisabledInit() {
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -14,14 +14,14 @@ Elevator::Elevator() {
// Put Mechanism 2d to SmartDashboard
// To view the Elevator visualization, select Network Tables -> SmartDashboard
// -> Elevator Sim
frc::SmartDashboard::PutData("Elevator Sim", &m_mech2d);
wpi::SmartDashboard::PutData("Elevator Sim", &m_mech2d);
}
void Elevator::SimulationPeriodic() {
// In this method, we update our simulation of what our elevator is doing
// First, we set our "inputs" (voltages)
m_elevatorSim.SetInput(frc::Vectord<1>{
m_motorSim.GetSpeed() * frc::RobotController::GetInputVoltage()});
m_elevatorSim.SetInput(wpi::math::Vectord<1>{
m_motorSim.GetSpeed() * wpi::RobotController::GetInputVoltage()});
// Next, we update it. The standard loop time is 20ms.
m_elevatorSim.Update(20_ms);
@@ -30,8 +30,8 @@ void Elevator::SimulationPeriodic() {
// voltage
m_encoderSim.SetDistance(m_elevatorSim.GetPosition().value());
// SimBattery estimates loaded battery voltages
frc::sim::RoboRioSim::SetVInVoltage(
frc::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()}));
wpi::sim::RoboRioSim::SetVInVoltage(
wpi::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()}));
}
void Elevator::UpdateTelemetry() {
@@ -39,14 +39,14 @@ void Elevator::UpdateTelemetry() {
m_elevatorMech2d->SetLength(m_encoder.GetDistance());
}
void Elevator::ReachGoal(units::meter_t goal) {
void Elevator::ReachGoal(wpi::units::meter_t goal) {
m_controller.SetGoal(goal);
// With the setpoint value we run PID control like normal
double pidOutput =
m_controller.Calculate(units::meter_t{m_encoder.GetDistance()});
units::volt_t feedforwardOutput =
m_controller.Calculate(wpi::units::meter_t{m_encoder.GetDistance()});
wpi::units::volt_t feedforwardOutput =
m_feedforward.Calculate(m_controller.GetSetpoint().velocity);
m_motor.SetVoltage(units::volt_t{pidOutput} + feedforwardOutput);
m_motor.SetVoltage(wpi::units::volt_t{pidOutput} + feedforwardOutput);
}
void Elevator::Stop() {

View File

@@ -34,18 +34,18 @@ inline constexpr double kElevatorKp = 5.0;
inline constexpr double kElevatorKi = 0.0;
inline constexpr double kElevatorKd = 0.0;
inline constexpr units::volt_t kElevatorkS = 0.0_V;
inline constexpr units::volt_t kElevatorkG = 0.762_V;
inline constexpr wpi::units::volt_t kElevatorkS = 0.0_V;
inline constexpr wpi::units::volt_t kElevatorkG = 0.762_V;
inline constexpr auto kElevatorkV = 0.762_V / 1_mps;
inline constexpr auto kElevatorkA = 0.0_V / 1_mps_sq;
inline constexpr double kElevatorGearing = 10.0;
inline constexpr units::meter_t kElevatorDrumRadius = 2_in;
inline constexpr units::kilogram_t kCarriageMass = 4.0_kg;
inline constexpr wpi::units::meter_t kElevatorDrumRadius = 2_in;
inline constexpr wpi::units::kilogram_t kCarriageMass = 4.0_kg;
inline constexpr units::meter_t kSetpoint = 75_cm;
inline constexpr units::meter_t kMinElevatorHeight = 0_cm;
inline constexpr units::meter_t kMaxElevatorHeight = 1.25_m;
inline constexpr wpi::units::meter_t kSetpoint = 75_cm;
inline constexpr wpi::units::meter_t kMinElevatorHeight = 0_cm;
inline constexpr wpi::units::meter_t kMaxElevatorHeight = 1.25_m;
// distance per pulse = (distance per revolution) / (pulses per revolution)
// = (Pi * D) / ppr

View File

@@ -12,7 +12,7 @@
/**
* This is a sample program to demonstrate the use of elevator simulation.
*/
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
Robot() {}
void RobotPeriodic() override;
@@ -21,6 +21,6 @@ class Robot : public frc::TimedRobot {
void DisabledInit() override;
private:
frc::Joystick m_joystick{Constants::kJoystickPort};
wpi::Joystick m_joystick{Constants::kJoystickPort};
Elevator m_elevator;
};

View File

@@ -26,30 +26,30 @@ class Elevator {
Elevator();
void SimulationPeriodic();
void UpdateTelemetry();
void ReachGoal(units::meter_t goal);
void ReachGoal(wpi::units::meter_t goal);
void Stop();
private:
// This gearbox represents a gearbox containing 4 Vex 775pro motors.
frc::DCMotor m_elevatorGearbox = frc::DCMotor::Vex775Pro(4);
wpi::math::DCMotor m_elevatorGearbox = wpi::math::DCMotor::Vex775Pro(4);
// Standard classes for controlling our elevator
frc::TrapezoidProfile<units::meters>::Constraints m_constraints{2.45_mps,
wpi::math::TrapezoidProfile<wpi::units::meters>::Constraints m_constraints{2.45_mps,
2.45_mps_sq};
frc::ProfiledPIDController<units::meters> m_controller{
wpi::math::ProfiledPIDController<wpi::units::meters> m_controller{
Constants::kElevatorKp, Constants::kElevatorKi, Constants::kElevatorKd,
m_constraints};
frc::ElevatorFeedforward m_feedforward{
wpi::math::ElevatorFeedforward m_feedforward{
Constants::kElevatorkS, Constants::kElevatorkG, Constants::kElevatorkV,
Constants::kElevatorkA};
frc::Encoder m_encoder{Constants::kEncoderAChannel,
wpi::Encoder m_encoder{Constants::kEncoderAChannel,
Constants::kEncoderBChannel};
frc::PWMSparkMax m_motor{Constants::kMotorPort};
frc::sim::PWMMotorControllerSim m_motorSim{m_motor};
wpi::PWMSparkMax m_motor{Constants::kMotorPort};
wpi::sim::PWMMotorControllerSim m_motorSim{m_motor};
// Simulation classes help us simulate what's going on, including gravity.
frc::sim::ElevatorSim m_elevatorSim{m_elevatorGearbox,
wpi::sim::ElevatorSim m_elevatorSim{m_elevatorGearbox,
Constants::kElevatorGearing,
Constants::kCarriageMass,
Constants::kElevatorDrumRadius,
@@ -58,13 +58,13 @@ class Elevator {
true,
0_m,
{0.01}};
frc::sim::EncoderSim m_encoderSim{m_encoder};
wpi::sim::EncoderSim m_encoderSim{m_encoder};
// Create a Mechanism2d display of an elevator
frc::Mechanism2d m_mech2d{20, 50};
frc::MechanismRoot2d* m_elevatorRoot =
wpi::Mechanism2d m_mech2d{20, 50};
wpi::MechanismRoot2d* m_elevatorRoot =
m_mech2d.GetRoot("Elevator Root", 10, 0);
frc::MechanismLigament2d* m_elevatorMech2d =
m_elevatorRoot->Append<frc::MechanismLigament2d>(
wpi::MechanismLigament2d* m_elevatorMech2d =
m_elevatorRoot->Append<wpi::MechanismLigament2d>(
"Elevator", m_elevatorSim.GetPosition().value(), 90_deg);
};

View File

@@ -16,9 +16,9 @@
#include "ExampleSmartMotorController.hpp"
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
static constexpr units::second_t kDt = 20_ms;
static constexpr wpi::units::second_t kDt = 20_ms;
Robot() {
// Note: These gains are fake, and will have to be tuned for your robot.
@@ -43,21 +43,21 @@ class Robot : public frc::TimedRobot {
}
private:
frc::Joystick m_joystick{1};
wpi::Joystick m_joystick{1};
ExampleSmartMotorController m_motor{1};
frc::SimpleMotorFeedforward<units::meters> m_feedforward{
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{
// Note: These gains are fake, and will have to be tuned for your robot.
1_V, 1.5_V * 1_s / 1_m};
// Create a motion profile with the given maximum velocity and maximum
// acceleration constraints for the next setpoint.
frc::TrapezoidProfile<units::meters> m_profile{{1.75_mps, 0.75_mps_sq}};
frc::TrapezoidProfile<units::meters>::State m_goal;
frc::TrapezoidProfile<units::meters>::State m_setpoint;
wpi::math::TrapezoidProfile<wpi::units::meters> m_profile{{1.75_mps, 0.75_mps_sq}};
wpi::math::TrapezoidProfile<wpi::units::meters>::State m_goal;
wpi::math::TrapezoidProfile<wpi::units::meters>::State m_setpoint;
};
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -27,7 +27,7 @@
* distance that the robot drives can be precisely controlled during the
* autonomous mode.
*/
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
Robot() {
/* Defines the number of samples to average when determining the rate.
@@ -53,10 +53,10 @@ class Robot : public frc::TimedRobot {
void TeleopPeriodic() override {
// Retrieve the net displacement of the Encoder since the last Reset.
frc::SmartDashboard::PutNumber("Encoder Distance", m_encoder.GetDistance());
wpi::SmartDashboard::PutNumber("Encoder Distance", m_encoder.GetDistance());
// Retrieve the current rate of the encoder.
frc::SmartDashboard::PutNumber("Encoder Rate", m_encoder.GetRate());
wpi::SmartDashboard::PutNumber("Encoder Rate", m_encoder.GetRate());
}
private:
@@ -76,11 +76,11 @@ class Robot : public frc::TimedRobot {
* and defaults to k4X. Faster (k4X) encoding gives greater positional
* precision but more noise in the rate.
*/
frc::Encoder m_encoder{1, 2, false, frc::Encoder::k4X};
wpi::Encoder m_encoder{1, 2, false, wpi::Encoder::k4X};
};
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -18,17 +18,17 @@
static const auto SHOT_VELOCITY = 200_rpm;
static const auto TOLERANCE = 8_rpm;
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
Robot() {
m_controller.SetTolerance(TOLERANCE.value());
frc::BooleanEvent isBallAtKicker{&m_loop, [] {
wpi::BooleanEvent isBallAtKicker{&m_loop, [] {
return false;
// return kickerSensor.GetRange() <
// KICKER_THRESHOLD;
}};
frc::BooleanEvent intakeButton{
wpi::BooleanEvent intakeButton{
&m_loop, [&joystick = m_joystick] { return joystick.GetRawButton(2); }};
// if the thumb button is held
@@ -45,7 +45,7 @@ class Robot : public frc::TimedRobot {
// stop the intake
.IfHigh([&intake = m_intake] { intake.Set(0.0); });
frc::BooleanEvent shootTrigger{
wpi::BooleanEvent shootTrigger{
&m_loop, [&joystick = m_joystick] { return joystick.GetTrigger(); }};
// if the trigger is held
@@ -54,15 +54,15 @@ class Robot : public frc::TimedRobot {
.IfHigh([&shooter = m_shooter, &controller = m_controller, &ff = m_ff,
&encoder = m_shooterEncoder] {
shooter.SetVoltage(
units::volt_t{controller.Calculate(encoder.GetRate(),
wpi::units::volt_t{controller.Calculate(encoder.GetRate(),
SHOT_VELOCITY.value())} +
ff.Calculate(units::radians_per_second_t{SHOT_VELOCITY}));
ff.Calculate(wpi::units::radians_per_second_t{SHOT_VELOCITY}));
});
// if not, stop
(!shootTrigger).IfHigh([&shooter = m_shooter] { shooter.Set(0.0); });
frc::BooleanEvent atTargetVelocity =
frc::BooleanEvent(
wpi::BooleanEvent atTargetVelocity =
wpi::BooleanEvent(
&m_loop,
[&controller = m_controller] { return controller.AtSetpoint(); })
// debounce for more stability
@@ -81,21 +81,21 @@ class Robot : public frc::TimedRobot {
void RobotPeriodic() override { m_loop.Poll(); }
private:
frc::PWMSparkMax m_shooter{0};
frc::Encoder m_shooterEncoder{0, 1};
frc::PIDController m_controller{0.3, 0, 0};
frc::SimpleMotorFeedforward<units::radians> m_ff{0.1_V, 0.065_V / 1_rpm};
wpi::PWMSparkMax m_shooter{0};
wpi::Encoder m_shooterEncoder{0, 1};
wpi::math::PIDController m_controller{0.3, 0, 0};
wpi::math::SimpleMotorFeedforward<wpi::units::radians> m_ff{0.1_V, 0.065_V / 1_rpm};
frc::PWMSparkMax m_kicker{1};
wpi::PWMSparkMax m_kicker{1};
frc::PWMSparkMax m_intake{3};
wpi::PWMSparkMax m_intake{3};
frc::EventLoop m_loop{};
frc::Joystick m_joystick{0};
wpi::EventLoop m_loop{};
wpi::Joystick m_joystick{0};
};
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -15,21 +15,21 @@
#include <wpi/units/moment_of_inertia.hpp>
/**
* This is a sample program to demonstrate the use of a BangBangController with
* This is a sample program to demonstrate the use of a wpi::math::BangBangController with
* a flywheel to control speed.
*/
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
/**
* Controls flywheel to a set speed (RPM) controlled by a joystick.
*/
void TeleopPeriodic() override {
// Scale setpoint value between 0 and maxSetpointValue
units::radians_per_second_t setpoint =
units::math::max(0_rpm, m_joystick.GetRawAxis(0) * kMaxSetpointValue);
wpi::units::radians_per_second_t setpoint =
wpi::units::math::max(0_rpm, m_joystick.GetRawAxis(0) * kMaxSetpointValue);
// Set setpoint and measurement of the bang-bang controller
units::volt_t bangOutput =
wpi::units::volt_t bangOutput =
m_bangBangController.Calculate(m_encoder.GetRate(), setpoint.value()) *
12_V;
@@ -42,7 +42,7 @@ class Robot : public frc::TimedRobot {
Robot() {
// Add bang-bang controller to SmartDashboard and networktables.
frc::SmartDashboard::PutData("BangBangController", &m_bangBangController);
wpi::SmartDashboard::PutData("BangBangController", &m_bangBangController);
}
/**
@@ -53,7 +53,7 @@ class Robot : public frc::TimedRobot {
// simulation, and write the simulated velocities to our simulated encoder
m_flywheelSim.SetInputVoltage(
m_flywheelMotor.Get() *
units::volt_t{frc::RobotController::GetInputVoltage()});
wpi::units::volt_t{wpi::RobotController::GetInputVoltage()});
m_flywheelSim.Update(20_ms);
m_encoderSim.SetRate(m_flywheelSim.GetAngularVelocity().value());
}
@@ -64,23 +64,23 @@ class Robot : public frc::TimedRobot {
static constexpr int kEncoderBChannel = 1;
// Max setpoint for joystick control
static constexpr units::radians_per_second_t kMaxSetpointValue = 6000_rpm;
static constexpr wpi::units::radians_per_second_t kMaxSetpointValue = 6000_rpm;
// Joystick to control setpoint
frc::Joystick m_joystick{0};
wpi::Joystick m_joystick{0};
frc::PWMSparkMax m_flywheelMotor{kMotorPort};
frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
wpi::PWMSparkMax m_flywheelMotor{kMotorPort};
wpi::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
frc::BangBangController m_bangBangController;
wpi::math::BangBangController m_bangBangController;
// Gains are for example purposes only - must be determined for your own
// robot!
static constexpr units::volt_t kFlywheelKs = 0.0001_V;
static constexpr wpi::units::volt_t kFlywheelKs = 0.0001_V;
static constexpr decltype(1_V / 1_rad_per_s) kFlywheelKv = 0.000195_V / 1_rpm;
static constexpr decltype(1_V / 1_rad_per_s_sq) kFlywheelKa =
0.0003_V / 1_rev_per_m_per_s;
frc::SimpleMotorFeedforward<units::radians> m_feedforward{
wpi::math::SimpleMotorFeedforward<wpi::units::radians> m_feedforward{
kFlywheelKs, kFlywheelKv, kFlywheelKa};
// Simulation classes help us simulate our robot
@@ -90,19 +90,19 @@ class Robot : public frc::TimedRobot {
static constexpr double kFlywheelGearing = 1.0;
// 1/2 MR²
static constexpr units::kilogram_square_meter_t kFlywheelMomentOfInertia =
static constexpr wpi::units::kilogram_square_meter_t kFlywheelMomentOfInertia =
0.5 * 1.5_lb * 4_in * 4_in;
frc::DCMotor m_gearbox = frc::DCMotor::NEO(1);
frc::LinearSystem<1, 1, 1> m_plant{frc::LinearSystemId::FlywheelSystem(
wpi::math::DCMotor m_gearbox = wpi::math::DCMotor::NEO(1);
wpi::math::LinearSystem<1, 1, 1> m_plant{wpi::math::LinearSystemId::FlywheelSystem(
m_gearbox, kFlywheelMomentOfInertia, kFlywheelGearing)};
frc::sim::FlywheelSim m_flywheelSim{m_plant, m_gearbox};
frc::sim::EncoderSim m_encoderSim{m_encoder};
wpi::sim::FlywheelSim m_flywheelSim{m_plant, m_gearbox};
wpi::sim::EncoderSim m_encoderSim{m_encoder};
};
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -8,11 +8,11 @@
#include <wpi/opmode/TimedRobot.hpp>
#include <wpi/system/Timer.hpp>
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
Robot() {
wpi::SendableRegistry::AddChild(&m_robotDrive, &m_left);
wpi::SendableRegistry::AddChild(&m_robotDrive, &m_right);
wpi::util::SendableRegistry::AddChild(&m_robotDrive, &m_left);
wpi::util::SendableRegistry::AddChild(&m_robotDrive, &m_right);
// We need to invert one side of the drivetrain so that positive voltages
// result in both sides moving forward. Depending on how your robot's
@@ -49,18 +49,18 @@ class Robot : public frc::TimedRobot {
private:
// Robot drive system
frc::PWMSparkMax m_left{0};
frc::PWMSparkMax m_right{1};
frc::DifferentialDrive m_robotDrive{
wpi::PWMSparkMax m_left{0};
wpi::PWMSparkMax m_right{1};
wpi::DifferentialDrive m_robotDrive{
[&](double output) { m_left.Set(output); },
[&](double output) { m_right.Set(output); }};
frc::XboxController m_controller{0};
frc::Timer m_timer;
wpi::XboxController m_controller{0};
wpi::Timer m_timer;
};
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -15,7 +15,7 @@
* robot drive straight. This program uses a joystick to drive forwards and
* backwards while the gyro is used for direction keeping.
*/
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
Robot() {
// We need to invert one side of the drivetrain so that positive voltages
@@ -23,8 +23,8 @@ class Robot : public frc::TimedRobot {
// gearbox is constructed, you might have to invert the left side instead.
m_right.SetInverted(true);
wpi::SendableRegistry::AddChild(&m_drive, &m_left);
wpi::SendableRegistry::AddChild(&m_drive, &m_right);
wpi::util::SendableRegistry::AddChild(&m_drive, &m_left);
wpi::util::SendableRegistry::AddChild(&m_drive, &m_right);
}
/**
@@ -44,21 +44,21 @@ class Robot : public frc::TimedRobot {
static constexpr int kLeftMotorPort = 0;
static constexpr int kRightMotorPort = 1;
static constexpr frc::OnboardIMU::MountOrientation kIMUMountOrientation =
frc::OnboardIMU::kFlat;
static constexpr wpi::OnboardIMU::MountOrientation kIMUMountOrientation =
wpi::OnboardIMU::kFlat;
static constexpr int kJoystickPort = 0;
frc::PWMSparkMax m_left{kLeftMotorPort};
frc::PWMSparkMax m_right{kRightMotorPort};
frc::DifferentialDrive m_drive{[&](double output) { m_left.Set(output); },
wpi::PWMSparkMax m_left{kLeftMotorPort};
wpi::PWMSparkMax m_right{kRightMotorPort};
wpi::DifferentialDrive m_drive{[&](double output) { m_left.Set(output); },
[&](double output) { m_right.Set(output); }};
frc::OnboardIMU m_imu{kIMUMountOrientation};
frc::Joystick m_joystick{kJoystickPort};
wpi::OnboardIMU m_imu{kIMUMountOrientation};
wpi::Joystick m_joystick{kJoystickPort};
};
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -13,13 +13,13 @@
* maintain rotation vectors in relation to the starting orientation of the
* robot (field-oriented controls).
*/
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
Robot() {
wpi::SendableRegistry::AddChild(&m_robotDrive, &m_frontLeft);
wpi::SendableRegistry::AddChild(&m_robotDrive, &m_rearLeft);
wpi::SendableRegistry::AddChild(&m_robotDrive, &m_frontRight);
wpi::SendableRegistry::AddChild(&m_robotDrive, &m_rearRight);
wpi::util::SendableRegistry::AddChild(&m_robotDrive, &m_frontLeft);
wpi::util::SendableRegistry::AddChild(&m_robotDrive, &m_rearLeft);
wpi::util::SendableRegistry::AddChild(&m_robotDrive, &m_frontRight);
wpi::util::SendableRegistry::AddChild(&m_robotDrive, &m_rearRight);
// Invert the right side motors. You may need to change or remove this to
// match your robot.
@@ -40,26 +40,26 @@ class Robot : public frc::TimedRobot {
static constexpr int kRearLeftMotorPort = 1;
static constexpr int kFrontRightMotorPort = 2;
static constexpr int kRearRightMotorPort = 3;
static constexpr frc::OnboardIMU::MountOrientation kIMUMountOrientation =
frc::OnboardIMU::kFlat;
static constexpr wpi::OnboardIMU::MountOrientation kIMUMountOrientation =
wpi::OnboardIMU::kFlat;
static constexpr int kJoystickPort = 0;
frc::PWMSparkMax m_frontLeft{kFrontLeftMotorPort};
frc::PWMSparkMax m_rearLeft{kRearLeftMotorPort};
frc::PWMSparkMax m_frontRight{kFrontRightMotorPort};
frc::PWMSparkMax m_rearRight{kRearRightMotorPort};
frc::MecanumDrive m_robotDrive{
wpi::PWMSparkMax m_frontLeft{kFrontLeftMotorPort};
wpi::PWMSparkMax m_rearLeft{kRearLeftMotorPort};
wpi::PWMSparkMax m_frontRight{kFrontRightMotorPort};
wpi::PWMSparkMax m_rearRight{kRearRightMotorPort};
wpi::MecanumDrive m_robotDrive{
[&](double output) { m_frontLeft.Set(output); },
[&](double output) { m_rearLeft.Set(output); },
[&](double output) { m_frontRight.Set(output); },
[&](double output) { m_rearRight.Set(output); }};
frc::OnboardIMU m_imu{kIMUMountOrientation};
frc::Joystick m_joystick{kJoystickPort};
wpi::OnboardIMU m_imu{kIMUMountOrientation};
wpi::Joystick m_joystick{kJoystickPort};
};
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -11,11 +11,11 @@
Robot::Robot() {
// Start recording to data log
frc::DataLogManager::Start();
wpi::DataLogManager::Start();
// Record DS control and joystick data.
// Change to `false` to not record joystick data.
frc::DriverStation::StartDataLog(frc::DataLogManager::GetLog(), true);
wpi::DriverStation::StartDataLog(wpi::DataLogManager::GetLog(), true);
}
/**
@@ -27,7 +27,7 @@ Robot::Robot() {
* LiveWindow and SmartDashboard integrated updating.
*/
void Robot::RobotPeriodic() {
frc2::CommandScheduler::GetInstance().Run();
wpi::cmd::CommandScheduler::GetInstance().Run();
}
/**
@@ -47,7 +47,7 @@ void Robot::AutonomousInit() {
m_autonomousCommand = m_container.GetAutonomousCommand();
if (m_autonomousCommand != nullptr) {
frc2::CommandScheduler::GetInstance().Schedule(m_autonomousCommand);
wpi::cmd::CommandScheduler::GetInstance().Schedule(m_autonomousCommand);
}
}
@@ -76,6 +76,6 @@ void Robot::TestPeriodic() {}
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -15,16 +15,16 @@ RobotContainer::RobotContainer() {
m_chooser.AddOption("Complex Auto", m_complexAuto.get());
// Put the chooser on the dashboard
frc::SmartDashboard::PutData("Autonomous", &m_chooser);
wpi::SmartDashboard::PutData("Autonomous", &m_chooser);
// Put subsystems to dashboard.
frc::SmartDashboard::PutData("Drivetrain", &m_drive);
frc::SmartDashboard::PutData("HatchSubsystem", &m_hatch);
wpi::SmartDashboard::PutData("Drivetrain", &m_drive);
wpi::SmartDashboard::PutData("HatchSubsystem", &m_hatch);
// Configure the button bindings
ConfigureButtonBindings();
// Set up default drive command
m_drive.SetDefaultCommand(frc2::cmd::Run(
m_drive.SetDefaultCommand(wpi::cmd::cmd::Run(
[this] {
m_drive.ArcadeDrive(-m_driverController.GetLeftY(),
-m_driverController.GetRightX());
@@ -41,11 +41,11 @@ void RobotContainer::ConfigureButtonBindings() {
m_driverController.Square().OnTrue(m_hatch.ReleaseHatchCommand());
// While holding R1, drive at half speed
m_driverController.R1()
.OnTrue(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(0.5); }, {}))
.OnFalse(frc2::cmd::RunOnce([this] { m_drive.SetMaxOutput(1.0); }, {}));
.OnTrue(wpi::cmd::cmd::RunOnce([this] { m_drive.SetMaxOutput(0.5); }, {}))
.OnFalse(wpi::cmd::cmd::RunOnce([this] { m_drive.SetMaxOutput(1.0); }, {}));
}
frc2::Command* RobotContainer::GetAutonomousCommand() {
wpi::cmd::Command* RobotContainer::GetAutonomousCommand() {
// Runs the chosen command in autonomous
return m_chooser.GetSelected();
}

View File

@@ -11,8 +11,8 @@
using namespace AutoConstants;
frc2::CommandPtr autos::SimpleAuto(DriveSubsystem* drive) {
return frc2::FunctionalCommand(
wpi::cmd::CommandPtr autos::SimpleAuto(DriveSubsystem* drive) {
return wpi::cmd::FunctionalCommand(
// Reset encoders on command start
[drive] { drive->ResetEncoders(); },
// Drive forward while the command is executing
@@ -30,11 +30,11 @@ frc2::CommandPtr autos::SimpleAuto(DriveSubsystem* drive) {
.ToPtr();
}
frc2::CommandPtr autos::ComplexAuto(DriveSubsystem* drive,
wpi::cmd::CommandPtr autos::ComplexAuto(DriveSubsystem* drive,
HatchSubsystem* hatch) {
return frc2::cmd::Sequence(
return wpi::cmd::cmd::Sequence(
// Drive forward the specified distance
frc2::FunctionalCommand(
wpi::cmd::FunctionalCommand(
// Reset encoders on command start
[drive] { drive->ResetEncoders(); },
// Drive forward while the command is executing
@@ -54,7 +54,7 @@ frc2::CommandPtr autos::ComplexAuto(DriveSubsystem* drive,
hatch->ReleaseHatchCommand(),
// Drive backward the specified distance
// Drive forward the specified distance
frc2::FunctionalCommand(
wpi::cmd::FunctionalCommand(
// Reset encoders on command start
[drive] { drive->ResetEncoders(); },
// Drive backward while the command is executing

View File

@@ -15,8 +15,8 @@ DriveSubsystem::DriveSubsystem()
m_right2{kRightMotor2Port},
m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
wpi::SendableRegistry::AddChild(&m_drive, &m_left1);
wpi::SendableRegistry::AddChild(&m_drive, &m_right1);
wpi::util::SendableRegistry::AddChild(&m_drive, &m_left1);
wpi::util::SendableRegistry::AddChild(&m_drive, &m_right1);
m_left1.AddFollower(m_left2);
m_right1.AddFollower(m_right2);
@@ -52,7 +52,7 @@ void DriveSubsystem::SetMaxOutput(double maxOutput) {
m_drive.SetMaxOutput(maxOutput);
}
void DriveSubsystem::InitSendable(wpi::SendableBuilder& builder) {
void DriveSubsystem::InitSendable(wpi::util::SendableBuilder& builder) {
SubsystemBase::InitSendable(builder);
// Publish encoder distances to telemetry.

View File

@@ -9,27 +9,27 @@
using namespace HatchConstants;
HatchSubsystem::HatchSubsystem()
: m_hatchSolenoid{0, frc::PneumaticsModuleType::CTREPCM,
: m_hatchSolenoid{0, wpi::PneumaticsModuleType::CTREPCM,
kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]} {}
frc2::CommandPtr HatchSubsystem::GrabHatchCommand() {
wpi::cmd::CommandPtr HatchSubsystem::GrabHatchCommand() {
// implicitly require `this`
return this->RunOnce(
[this] { m_hatchSolenoid.Set(frc::DoubleSolenoid::kForward); });
[this] { m_hatchSolenoid.Set(wpi::DoubleSolenoid::kForward); });
}
frc2::CommandPtr HatchSubsystem::ReleaseHatchCommand() {
wpi::cmd::CommandPtr HatchSubsystem::ReleaseHatchCommand() {
// implicitly require `this`
return this->RunOnce(
[this] { m_hatchSolenoid.Set(frc::DoubleSolenoid::kReverse); });
[this] { m_hatchSolenoid.Set(wpi::DoubleSolenoid::kReverse); });
}
void HatchSubsystem::InitSendable(wpi::SendableBuilder& builder) {
void HatchSubsystem::InitSendable(wpi::util::SendableBuilder& builder) {
SubsystemBase::InitSendable(builder);
// Publish the solenoid state to telemetry.
builder.AddBooleanProperty(
"extended",
[this] { return m_hatchSolenoid.Get() == frc::DoubleSolenoid::kForward; },
[this] { return m_hatchSolenoid.Get() == wpi::DoubleSolenoid::kForward; },
nullptr);
}

View File

@@ -9,7 +9,7 @@
#include "RobotContainer.hpp"
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
Robot();
void RobotPeriodic() override;
@@ -24,7 +24,7 @@ class Robot : public frc::TimedRobot {
private:
// Have it null by default so that if testing teleop it
// doesn't have undefined behavior and potentially crash.
frc2::Command* m_autonomousCommand = nullptr;
wpi::cmd::Command* m_autonomousCommand = nullptr;
RobotContainer m_container;
};

View File

@@ -27,11 +27,11 @@ class RobotContainer {
public:
RobotContainer();
frc2::Command* GetAutonomousCommand();
wpi::cmd::Command* GetAutonomousCommand();
private:
// The driver's controller
frc2::CommandPS4Controller m_driverController{
wpi::cmd::CommandPS4Controller m_driverController{
OIConstants::kDriverControllerPort};
// The robot's subsystems and commands are defined here...
@@ -43,11 +43,11 @@ class RobotContainer {
// Commands owned by RobotContainer
// The autonomous routines
frc2::CommandPtr m_simpleAuto = autos::SimpleAuto(&m_drive);
frc2::CommandPtr m_complexAuto = autos::ComplexAuto(&m_drive, &m_hatch);
wpi::cmd::CommandPtr m_simpleAuto = autos::SimpleAuto(&m_drive);
wpi::cmd::CommandPtr m_complexAuto = autos::ComplexAuto(&m_drive, &m_hatch);
// The chooser for the autonomous routines
frc::SendableChooser<frc2::Command*> m_chooser;
wpi::SendableChooser<wpi::cmd::Command*> m_chooser;
void ConfigureButtonBindings();
};

View File

@@ -15,12 +15,12 @@ namespace autos {
/**
* A simple auto that drives forward, then stops.
*/
frc2::CommandPtr SimpleAuto(DriveSubsystem* drive);
wpi::cmd::CommandPtr SimpleAuto(DriveSubsystem* drive);
/**
* A complex auto command that drives forward, releases a hatch, and then drives
* backward.
*/
frc2::CommandPtr ComplexAuto(DriveSubsystem* drive, HatchSubsystem* hatch);
wpi::cmd::CommandPtr ComplexAuto(DriveSubsystem* drive, HatchSubsystem* hatch);
} // namespace autos

View File

@@ -11,7 +11,7 @@
#include "Constants.hpp"
class DriveSubsystem : public frc2::SubsystemBase {
class DriveSubsystem : public wpi::cmd::SubsystemBase {
public:
DriveSubsystem();
@@ -50,25 +50,25 @@ class DriveSubsystem : public frc2::SubsystemBase {
*/
void SetMaxOutput(double maxOutput);
void InitSendable(wpi::SendableBuilder& builder) override;
void InitSendable(wpi::util::SendableBuilder& builder) override;
private:
// Components (e.g. motor controllers and sensors) should generally be
// declared private and exposed only through public methods.
// The motor controllers
frc::PWMSparkMax m_left1;
frc::PWMSparkMax m_left2;
frc::PWMSparkMax m_right1;
frc::PWMSparkMax m_right2;
wpi::PWMSparkMax m_left1;
wpi::PWMSparkMax m_left2;
wpi::PWMSparkMax m_right1;
wpi::PWMSparkMax m_right2;
// The robot's drive
frc::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
wpi::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
[&](double output) { m_right1.Set(output); }};
// The left-side drive encoder
frc::Encoder m_leftEncoder;
wpi::Encoder m_leftEncoder;
// The right-side drive encoder
frc::Encoder m_rightEncoder;
wpi::Encoder m_rightEncoder;
};

View File

@@ -11,7 +11,7 @@
#include "Constants.hpp"
class HatchSubsystem : public frc2::SubsystemBase {
class HatchSubsystem : public wpi::cmd::SubsystemBase {
public:
HatchSubsystem();
@@ -20,17 +20,17 @@ class HatchSubsystem : public frc2::SubsystemBase {
/**
* Grabs the hatch.
*/
frc2::CommandPtr GrabHatchCommand();
wpi::cmd::CommandPtr GrabHatchCommand();
/**
* Releases the hatch.
*/
frc2::CommandPtr ReleaseHatchCommand();
wpi::cmd::CommandPtr ReleaseHatchCommand();
void InitSendable(wpi::SendableBuilder& builder) override;
void InitSendable(wpi::util::SendableBuilder& builder) override;
private:
// Components (e.g. motor controllers and sensors) should generally be
// declared private and exposed only through public methods.
frc::DoubleSolenoid m_hatchSolenoid;
wpi::DoubleSolenoid m_hatchSolenoid;
};

View File

@@ -11,11 +11,11 @@
Robot::Robot() {
// Start recording to data log
frc::DataLogManager::Start();
wpi::DataLogManager::Start();
// Record DS control and joystick data.
// Change to `false` to not record joystick data.
frc::DriverStation::StartDataLog(frc::DataLogManager::GetLog(), true);
wpi::DriverStation::StartDataLog(wpi::DataLogManager::GetLog(), true);
}
/**
@@ -27,7 +27,7 @@ Robot::Robot() {
* LiveWindow and SmartDashboard integrated updating.
*/
void Robot::RobotPeriodic() {
frc2::CommandScheduler::GetInstance().Run();
wpi::cmd::CommandScheduler::GetInstance().Run();
}
/**
@@ -47,7 +47,7 @@ void Robot::AutonomousInit() {
m_autonomousCommand = m_container.GetAutonomousCommand();
if (m_autonomousCommand != nullptr) {
frc2::CommandScheduler::GetInstance().Schedule(m_autonomousCommand);
wpi::cmd::CommandScheduler::GetInstance().Schedule(m_autonomousCommand);
}
}
@@ -76,6 +76,6 @@ void Robot::TestPeriodic() {}
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -20,10 +20,10 @@ RobotContainer::RobotContainer() {
m_chooser.AddOption("Complex Auto", &m_complexAuto);
// Put the chooser on the dashboard
frc::SmartDashboard::PutData("Autonomous", &m_chooser);
wpi::SmartDashboard::PutData("Autonomous", &m_chooser);
// Put subsystems to dashboard.
frc::SmartDashboard::PutData("Drivetrain", &m_drive);
frc::SmartDashboard::PutData("HatchSubsystem", &m_hatch);
wpi::SmartDashboard::PutData("Drivetrain", &m_drive);
wpi::SmartDashboard::PutData("HatchSubsystem", &m_hatch);
// Configure the button bindings
ConfigureButtonBindings();
@@ -41,18 +41,18 @@ void RobotContainer::ConfigureButtonBindings() {
// the scheduler thus, no memory leaks!
// Grab the hatch when the 'A' button is pressed.
frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kA)
wpi::cmd::JoystickButton(&m_driverController, wpi::XboxController::Button::kA)
.OnTrue(GrabHatch(&m_hatch).ToPtr());
// Release the hatch when the 'B' button is pressed.
frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kB)
wpi::cmd::JoystickButton(&m_driverController, wpi::XboxController::Button::kB)
.OnTrue(ReleaseHatch(&m_hatch).ToPtr());
// While holding the shoulder button, drive at half speed
frc2::JoystickButton(&m_driverController,
frc::XboxController::Button::kRightBumper)
wpi::cmd::JoystickButton(&m_driverController,
wpi::XboxController::Button::kRightBumper)
.WhileTrue(HalveDriveSpeed(&m_drive).ToPtr());
}
frc2::Command* RobotContainer::GetAutonomousCommand() {
wpi::cmd::Command* RobotContainer::GetAutonomousCommand() {
// Runs the chosen command in autonomous
return m_chooser.GetSelected();
}

View File

@@ -15,8 +15,8 @@ DriveSubsystem::DriveSubsystem()
m_right2{kRightMotor2Port},
m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
wpi::SendableRegistry::AddChild(&m_drive, &m_left1);
wpi::SendableRegistry::AddChild(&m_drive, &m_right1);
wpi::util::SendableRegistry::AddChild(&m_drive, &m_left1);
wpi::util::SendableRegistry::AddChild(&m_drive, &m_right1);
m_left1.AddFollower(m_left2);
m_right1.AddFollower(m_right2);
@@ -52,7 +52,7 @@ void DriveSubsystem::SetMaxOutput(double maxOutput) {
m_drive.SetMaxOutput(maxOutput);
}
void DriveSubsystem::InitSendable(wpi::SendableBuilder& builder) {
void DriveSubsystem::InitSendable(wpi::util::SendableBuilder& builder) {
SubsystemBase::InitSendable(builder);
// Publish encoder distances to telemetry.

View File

@@ -9,23 +9,23 @@
using namespace HatchConstants;
HatchSubsystem::HatchSubsystem()
: m_hatchSolenoid{0, frc::PneumaticsModuleType::CTREPCM,
: m_hatchSolenoid{0, wpi::PneumaticsModuleType::CTREPCM,
kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]} {}
void HatchSubsystem::GrabHatch() {
m_hatchSolenoid.Set(frc::DoubleSolenoid::kForward);
m_hatchSolenoid.Set(wpi::DoubleSolenoid::kForward);
}
void HatchSubsystem::ReleaseHatch() {
m_hatchSolenoid.Set(frc::DoubleSolenoid::kReverse);
m_hatchSolenoid.Set(wpi::DoubleSolenoid::kReverse);
}
void HatchSubsystem::InitSendable(wpi::SendableBuilder& builder) {
void HatchSubsystem::InitSendable(wpi::util::SendableBuilder& builder) {
SubsystemBase::InitSendable(builder);
// Publish the solenoid state to telemetry.
builder.AddBooleanProperty(
"extended",
[this] { return m_hatchSolenoid.Get() == frc::DoubleSolenoid::kForward; },
[this] { return m_hatchSolenoid.Get() == wpi::DoubleSolenoid::kForward; },
nullptr);
}

View File

@@ -9,7 +9,7 @@
#include "RobotContainer.hpp"
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
Robot();
void RobotPeriodic() override;
@@ -24,7 +24,7 @@ class Robot : public frc::TimedRobot {
private:
// Have it null by default so that if testing teleop it
// doesn't have undefined behavior and potentially crash.
frc2::Command* m_autonomousCommand = nullptr;
wpi::cmd::Command* m_autonomousCommand = nullptr;
RobotContainer m_container;
};

View File

@@ -26,7 +26,7 @@ class RobotContainer {
public:
RobotContainer();
frc2::Command* GetAutonomousCommand();
wpi::cmd::Command* GetAutonomousCommand();
private:
// The robot's subsystems and commands are defined here...
@@ -41,10 +41,10 @@ class RobotContainer {
ComplexAuto m_complexAuto{&m_drive, &m_hatch};
// The chooser for the autonomous routines
frc::SendableChooser<frc2::Command*> m_chooser;
wpi::SendableChooser<wpi::cmd::Command*> m_chooser;
// The driver's controller
frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
wpi::XboxController m_driverController{OIConstants::kDriverControllerPort};
void ConfigureButtonBindings();
};

View File

@@ -16,7 +16,7 @@
* backward.
*/
class ComplexAuto
: public frc2::CommandHelper<frc2::SequentialCommandGroup, ComplexAuto> {
: public wpi::cmd::CommandHelper<wpi::cmd::SequentialCommandGroup, ComplexAuto> {
public:
/**
* Creates a new ComplexAuto.

View File

@@ -18,7 +18,7 @@
*
* @see RunCommand
*/
class DefaultDrive : public frc2::CommandHelper<frc2::Command, DefaultDrive> {
class DefaultDrive : public wpi::cmd::CommandHelper<wpi::cmd::Command, DefaultDrive> {
public:
/**
* Creates a new DefaultDrive.

View File

@@ -9,7 +9,7 @@
#include "subsystems/DriveSubsystem.hpp"
class DriveDistance : public frc2::CommandHelper<frc2::Command, DriveDistance> {
class DriveDistance : public wpi::cmd::CommandHelper<wpi::cmd::Command, DriveDistance> {
public:
/**
* Creates a new DriveDistance.

View File

@@ -16,7 +16,7 @@
*
* @see InstantCommand
*/
class GrabHatch : public frc2::CommandHelper<frc2::Command, GrabHatch> {
class GrabHatch : public wpi::cmd::CommandHelper<wpi::cmd::Command, GrabHatch> {
public:
explicit GrabHatch(HatchSubsystem* subsystem);

View File

@@ -10,7 +10,7 @@
#include "subsystems/DriveSubsystem.hpp"
class HalveDriveSpeed
: public frc2::CommandHelper<frc2::Command, HalveDriveSpeed> {
: public wpi::cmd::CommandHelper<wpi::cmd::Command, HalveDriveSpeed> {
public:
explicit HalveDriveSpeed(DriveSubsystem* subsystem);

View File

@@ -16,7 +16,7 @@
*
* @see InstantCommand
*/
class ReleaseHatch : public frc2::CommandHelper<frc2::Command, ReleaseHatch> {
class ReleaseHatch : public wpi::cmd::CommandHelper<wpi::cmd::Command, ReleaseHatch> {
public:
explicit ReleaseHatch(HatchSubsystem* subsystem);

View File

@@ -11,7 +11,7 @@
#include "Constants.hpp"
class DriveSubsystem : public frc2::SubsystemBase {
class DriveSubsystem : public wpi::cmd::SubsystemBase {
public:
DriveSubsystem();
@@ -50,25 +50,25 @@ class DriveSubsystem : public frc2::SubsystemBase {
*/
void SetMaxOutput(double maxOutput);
void InitSendable(wpi::SendableBuilder& builder) override;
void InitSendable(wpi::util::SendableBuilder& builder) override;
private:
// Components (e.g. motor controllers and sensors) should generally be
// declared private and exposed only through public methods.
// The motor controllers
frc::PWMSparkMax m_left1;
frc::PWMSparkMax m_left2;
frc::PWMSparkMax m_right1;
frc::PWMSparkMax m_right2;
wpi::PWMSparkMax m_left1;
wpi::PWMSparkMax m_left2;
wpi::PWMSparkMax m_right1;
wpi::PWMSparkMax m_right2;
// The robot's drive
frc::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
wpi::DifferentialDrive m_drive{[&](double output) { m_left1.Set(output); },
[&](double output) { m_right1.Set(output); }};
// The left-side drive encoder
frc::Encoder m_leftEncoder;
wpi::Encoder m_leftEncoder;
// The right-side drive encoder
frc::Encoder m_rightEncoder;
wpi::Encoder m_rightEncoder;
};

View File

@@ -10,7 +10,7 @@
#include "Constants.hpp"
class HatchSubsystem : public frc2::SubsystemBase {
class HatchSubsystem : public wpi::cmd::SubsystemBase {
public:
HatchSubsystem();
@@ -26,10 +26,10 @@ class HatchSubsystem : public frc2::SubsystemBase {
*/
void ReleaseHatch();
void InitSendable(wpi::SendableBuilder& builder) override;
void InitSendable(wpi::util::SendableBuilder& builder) override;
private:
// Components (e.g. motor controllers and sensors) should generally be
// declared private and exposed only through public methods.
frc::DoubleSolenoid m_hatchSolenoid;
wpi::DoubleSolenoid m_hatchSolenoid;
};

View File

@@ -8,26 +8,26 @@
/**
* This is a demo program showing the use of GenericHID's rumble feature.
*/
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
void AutonomousInit() override {
// Turn on rumble at the start of auto
m_hid.SetRumble(frc::GenericHID::RumbleType::kLeftRumble, 1.0);
m_hid.SetRumble(frc::GenericHID::RumbleType::kRightRumble, 1.0);
m_hid.SetRumble(wpi::GenericHID::RumbleType::kLeftRumble, 1.0);
m_hid.SetRumble(wpi::GenericHID::RumbleType::kRightRumble, 1.0);
}
void DisabledInit() override {
// Stop the rumble when entering disabled
m_hid.SetRumble(frc::GenericHID::RumbleType::kLeftRumble, 0.0);
m_hid.SetRumble(frc::GenericHID::RumbleType::kRightRumble, 0.0);
m_hid.SetRumble(wpi::GenericHID::RumbleType::kLeftRumble, 0.0);
m_hid.SetRumble(wpi::GenericHID::RumbleType::kRightRumble, 0.0);
}
private:
frc::GenericHID m_hid{0};
wpi::GenericHID m_hid{0};
};
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -14,7 +14,7 @@
* and sent to the dashboard. OpenCV has many methods for different types of
* processing.
*/
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
Robot() {
// We need to run our vision program in a separate thread. If not, our robot
@@ -27,17 +27,17 @@ class Robot : public frc::TimedRobot {
static void VisionThread() {
// Create an HTTP camera. The address will need to be modified to have the
// correct team number. The exact path will depend on the source.
cs::HttpCamera camera{"My Camera", "http://10.x.y.11/video/stream.mjpg"};
wpi::cs::HttpCamera camera{"My Camera", "http://10.x.y.11/video/stream.mjpg"};
// Start capturing images
frc::CameraServer::StartAutomaticCapture(camera);
wpi::CameraServer::StartAutomaticCapture(camera);
// Set the resolution
camera.SetResolution(640, 480);
// Get a CvSink. This will capture Mats from the Camera
cs::CvSink cvSink = frc::CameraServer::GetVideo();
wpi::cs::CvSink cvSink = wpi::CameraServer::GetVideo();
// Setup a CvSource. This will send images back to the Dashboard
cs::CvSource outputStream =
frc::CameraServer::PutVideo("Rectangle", 640, 480);
wpi::cs::CvSource outputStream =
wpi::CameraServer::PutVideo("Rectangle", 640, 480);
// Mats are very memory expensive. Lets reuse this Mat.
cv::Mat mat;
@@ -62,6 +62,6 @@ class Robot : public frc::TimedRobot {
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -22,9 +22,9 @@ void Robot::RobotPeriodic() {
// alliance, enabled in teleop mode, with 43 seconds left in the match.
std::string allianceString = "U";
auto alliance = frc::DriverStation::GetAlliance();
auto alliance = wpi::DriverStation::GetAlliance();
if (alliance.has_value()) {
if (alliance == frc::DriverStation::Alliance::kRed) {
if (alliance == wpi::DriverStation::Alliance::kRed) {
allianceString = "R";
} else {
allianceString = "B";
@@ -33,15 +33,15 @@ void Robot::RobotPeriodic() {
auto string =
fmt::format("{}{}{}{:03}", allianceString,
frc::DriverStation::IsEnabled() ? "E" : "D",
frc::DriverStation::IsAutonomous() ? "A" : "T",
static_cast<int>(frc::Timer::GetMatchTime().value()));
wpi::DriverStation::IsEnabled() ? "E" : "D",
wpi::DriverStation::IsAutonomous() ? "A" : "T",
static_cast<int>(wpi::Timer::GetMatchTime().value()));
arduino.WriteBulk(reinterpret_cast<uint8_t*>(string.data()), string.size());
}
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -13,13 +13,13 @@
* This is a sample program demonstrating how to communicate to a light
* controller from the robot code using the roboRIO's I2C port.
*/
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
void RobotPeriodic() override;
static constexpr frc::I2C::Port kPort = frc::I2C::Port::kPort0;
static constexpr wpi::I2C::Port kPort = wpi::I2C::Port::kPort0;
private:
static constexpr int deviceAddress = 4;
frc::I2C arduino{kPort, deviceAddress};
wpi::I2C arduino{kPort, deviceAddress};
};

View File

@@ -17,7 +17,7 @@
* and sent to the dashboard. OpenCV has many methods for different types of
* processing.
*/
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
Robot() {
// We need to run our vision program in a separate thread. If not, our robot
@@ -36,15 +36,15 @@ class Robot : public frc::TimedRobot {
private:
static void VisionThread() {
// Get the USB camera from CameraServer
cs::UsbCamera camera = frc::CameraServer::StartAutomaticCapture();
wpi::cs::UsbCamera camera = wpi::CameraServer::StartAutomaticCapture();
// Set the resolution
camera.SetResolution(640, 480);
// Get a CvSink. This will capture Mats from the Camera
cs::CvSink cvSink = frc::CameraServer::GetVideo();
wpi::cs::CvSink cvSink = wpi::CameraServer::GetVideo();
// Setup a CvSource. This will send images back to the Dashboard
cs::CvSource outputStream =
frc::CameraServer::PutVideo("Rectangle", 640, 480);
wpi::cs::CvSource outputStream =
wpi::CameraServer::PutVideo("Rectangle", 640, 480);
// Mats are very memory expensive. Lets reuse this Mat.
cv::Mat mat;
@@ -72,6 +72,6 @@ class Robot : public frc::TimedRobot {
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -6,21 +6,21 @@
#include <wpi/math/kinematics/ChassisSpeeds.hpp>
frc::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const {
return {units::meters_per_second_t{m_frontLeftEncoder.GetRate()},
units::meters_per_second_t{m_frontRightEncoder.GetRate()},
units::meters_per_second_t{m_backLeftEncoder.GetRate()},
units::meters_per_second_t{m_backRightEncoder.GetRate()}};
wpi::math::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const {
return {wpi::units::meters_per_second_t{m_frontLeftEncoder.GetRate()},
wpi::units::meters_per_second_t{m_frontRightEncoder.GetRate()},
wpi::units::meters_per_second_t{m_backLeftEncoder.GetRate()},
wpi::units::meters_per_second_t{m_backRightEncoder.GetRate()}};
}
frc::MecanumDriveWheelPositions Drivetrain::GetCurrentWheelDistances() const {
return {units::meter_t{m_frontLeftEncoder.GetDistance()},
units::meter_t{m_frontRightEncoder.GetDistance()},
units::meter_t{m_backLeftEncoder.GetDistance()},
units::meter_t{m_backRightEncoder.GetDistance()}};
wpi::math::MecanumDriveWheelPositions Drivetrain::GetCurrentWheelDistances() const {
return {wpi::units::meter_t{m_frontLeftEncoder.GetDistance()},
wpi::units::meter_t{m_frontRightEncoder.GetDistance()},
wpi::units::meter_t{m_backLeftEncoder.GetDistance()},
wpi::units::meter_t{m_backRightEncoder.GetDistance()}};
}
void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) {
void Drivetrain::SetSpeeds(const wpi::math::MecanumDriveWheelSpeeds& wheelSpeeds) {
const auto frontLeftFeedforward =
m_feedforward.Calculate(wheelSpeeds.frontLeft);
const auto frontRightFeedforward =
@@ -39,21 +39,21 @@ void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) {
const double backRightOutput = m_backRightPIDController.Calculate(
m_backRightEncoder.GetRate(), wheelSpeeds.rearRight.value());
m_frontLeftMotor.SetVoltage(units::volt_t{frontLeftOutput} +
m_frontLeftMotor.SetVoltage(wpi::units::volt_t{frontLeftOutput} +
frontLeftFeedforward);
m_frontRightMotor.SetVoltage(units::volt_t{frontRightOutput} +
m_frontRightMotor.SetVoltage(wpi::units::volt_t{frontRightOutput} +
frontRightFeedforward);
m_backLeftMotor.SetVoltage(units::volt_t{backLeftOutput} +
m_backLeftMotor.SetVoltage(wpi::units::volt_t{backLeftOutput} +
backLeftFeedforward);
m_backRightMotor.SetVoltage(units::volt_t{backRightOutput} +
m_backRightMotor.SetVoltage(wpi::units::volt_t{backRightOutput} +
backRightFeedforward);
}
void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed,
units::radians_per_second_t rot, bool fieldRelative,
units::second_t period) {
frc::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot};
void Drivetrain::Drive(wpi::units::meters_per_second_t xSpeed,
wpi::units::meters_per_second_t ySpeed,
wpi::units::radians_per_second_t rot, bool fieldRelative,
wpi::units::second_t period) {
wpi::math::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot};
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.ToRobotRelative(m_imu.GetRotation2d());
}

View File

@@ -8,7 +8,7 @@
#include "Drivetrain.hpp"
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
void AutonomousPeriodic() override {
DriveWithJoystick(false);
@@ -18,14 +18,14 @@ class Robot : public frc::TimedRobot {
void TeleopPeriodic() override { DriveWithJoystick(true); }
private:
frc::XboxController m_controller{0};
wpi::XboxController m_controller{0};
Drivetrain m_mecanum;
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
// to 1.
frc::SlewRateLimiter<units::scalar> m_xspeedLimiter{3 / 1_s};
frc::SlewRateLimiter<units::scalar> m_yspeedLimiter{3 / 1_s};
frc::SlewRateLimiter<units::scalar> m_rotLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_xspeedLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_yspeedLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_rotLimiter{3 / 1_s};
void DriveWithJoystick(bool fieldRelative) {
// Get the x speed. We are inverting this because Xbox controllers return
@@ -52,6 +52,6 @@ class Robot : public frc::TimedRobot {
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -30,50 +30,50 @@ class Drivetrain {
m_backRightMotor.SetInverted(true);
}
frc::MecanumDriveWheelSpeeds GetCurrentState() const;
frc::MecanumDriveWheelPositions GetCurrentWheelDistances() const;
void SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds);
void Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
bool fieldRelative, units::second_t period);
wpi::math::MecanumDriveWheelSpeeds GetCurrentState() const;
wpi::math::MecanumDriveWheelPositions GetCurrentWheelDistances() const;
void SetSpeeds(const wpi::math::MecanumDriveWheelSpeeds& wheelSpeeds);
void Drive(wpi::units::meters_per_second_t xSpeed,
wpi::units::meters_per_second_t ySpeed, wpi::units::radians_per_second_t rot,
bool fieldRelative, wpi::units::second_t period);
void UpdateOdometry();
static constexpr units::meters_per_second_t kMaxSpeed =
static constexpr wpi::units::meters_per_second_t kMaxSpeed =
3.0_mps; // 3 meters per second
static constexpr units::radians_per_second_t kMaxAngularSpeed{
static constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{
std::numbers::pi}; // 1/2 rotation per second
private:
frc::PWMSparkMax m_frontLeftMotor{1};
frc::PWMSparkMax m_frontRightMotor{2};
frc::PWMSparkMax m_backLeftMotor{3};
frc::PWMSparkMax m_backRightMotor{4};
wpi::PWMSparkMax m_frontLeftMotor{1};
wpi::PWMSparkMax m_frontRightMotor{2};
wpi::PWMSparkMax m_backLeftMotor{3};
wpi::PWMSparkMax m_backRightMotor{4};
frc::Encoder m_frontLeftEncoder{0, 1};
frc::Encoder m_frontRightEncoder{2, 3};
frc::Encoder m_backLeftEncoder{4, 5};
frc::Encoder m_backRightEncoder{6, 7};
wpi::Encoder m_frontLeftEncoder{0, 1};
wpi::Encoder m_frontRightEncoder{2, 3};
wpi::Encoder m_backLeftEncoder{4, 5};
wpi::Encoder m_backRightEncoder{6, 7};
frc::Translation2d m_frontLeftLocation{0.381_m, 0.381_m};
frc::Translation2d m_frontRightLocation{0.381_m, -0.381_m};
frc::Translation2d m_backLeftLocation{-0.381_m, 0.381_m};
frc::Translation2d m_backRightLocation{-0.381_m, -0.381_m};
wpi::math::Translation2d m_frontLeftLocation{0.381_m, 0.381_m};
wpi::math::Translation2d m_frontRightLocation{0.381_m, -0.381_m};
wpi::math::Translation2d m_backLeftLocation{-0.381_m, 0.381_m};
wpi::math::Translation2d m_backRightLocation{-0.381_m, -0.381_m};
frc::PIDController m_frontLeftPIDController{1.0, 0.0, 0.0};
frc::PIDController m_frontRightPIDController{1.0, 0.0, 0.0};
frc::PIDController m_backLeftPIDController{1.0, 0.0, 0.0};
frc::PIDController m_backRightPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController m_frontLeftPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController m_frontRightPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController m_backLeftPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController m_backRightPIDController{1.0, 0.0, 0.0};
frc::OnboardIMU m_imu{frc::OnboardIMU::kFlat};
wpi::OnboardIMU m_imu{wpi::OnboardIMU::kFlat};
frc::MecanumDriveKinematics m_kinematics{
wpi::math::MecanumDriveKinematics m_kinematics{
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
m_backRightLocation};
frc::MecanumDriveOdometry m_odometry{m_kinematics, m_imu.GetRotation2d(),
wpi::math::MecanumDriveOdometry m_odometry{m_kinematics, m_imu.GetRotation2d(),
GetCurrentWheelDistances()};
// Gains are for example purposes only - must be determined for your own
// robot!
frc::SimpleMotorFeedforward<units::meters> m_feedforward{1_V, 3_V / 1_mps};
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{1_V, 3_V / 1_mps};
};

View File

@@ -11,13 +11,13 @@
* This is a demo program showing how to use Mecanum control with the
* MecanumDrive class.
*/
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
Robot() {
wpi::SendableRegistry::AddChild(&m_robotDrive, &m_frontLeft);
wpi::SendableRegistry::AddChild(&m_robotDrive, &m_rearLeft);
wpi::SendableRegistry::AddChild(&m_robotDrive, &m_frontRight);
wpi::SendableRegistry::AddChild(&m_robotDrive, &m_rearRight);
wpi::util::SendableRegistry::AddChild(&m_robotDrive, &m_frontLeft);
wpi::util::SendableRegistry::AddChild(&m_robotDrive, &m_rearLeft);
wpi::util::SendableRegistry::AddChild(&m_robotDrive, &m_frontRight);
wpi::util::SendableRegistry::AddChild(&m_robotDrive, &m_rearRight);
// Invert the right side motors. You may need to change or remove this to
// match your robot.
@@ -41,21 +41,21 @@ class Robot : public frc::TimedRobot {
static constexpr int kJoystickChannel = 0;
frc::PWMSparkMax m_frontLeft{kFrontLeftChannel};
frc::PWMSparkMax m_rearLeft{kRearLeftChannel};
frc::PWMSparkMax m_frontRight{kFrontRightChannel};
frc::PWMSparkMax m_rearRight{kRearRightChannel};
frc::MecanumDrive m_robotDrive{
wpi::PWMSparkMax m_frontLeft{kFrontLeftChannel};
wpi::PWMSparkMax m_rearLeft{kRearLeftChannel};
wpi::PWMSparkMax m_frontRight{kFrontRightChannel};
wpi::PWMSparkMax m_rearRight{kRearRightChannel};
wpi::MecanumDrive m_robotDrive{
[&](double output) { m_frontLeft.Set(output); },
[&](double output) { m_rearLeft.Set(output); },
[&](double output) { m_frontRight.Set(output); },
[&](double output) { m_rearRight.Set(output); }};
frc::Joystick m_stick{kJoystickChannel};
wpi::Joystick m_stick{kJoystickChannel};
};
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -8,30 +8,30 @@
#include "ExampleGlobalMeasurementSensor.hpp"
frc::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const {
return {units::meters_per_second_t{m_frontLeftEncoder.GetRate()},
units::meters_per_second_t{m_frontRightEncoder.GetRate()},
units::meters_per_second_t{m_backLeftEncoder.GetRate()},
units::meters_per_second_t{m_backRightEncoder.GetRate()}};
wpi::math::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const {
return {wpi::units::meters_per_second_t{m_frontLeftEncoder.GetRate()},
wpi::units::meters_per_second_t{m_frontRightEncoder.GetRate()},
wpi::units::meters_per_second_t{m_backLeftEncoder.GetRate()},
wpi::units::meters_per_second_t{m_backRightEncoder.GetRate()}};
}
frc::MecanumDriveWheelPositions Drivetrain::GetCurrentDistances() const {
return {units::meter_t{m_frontLeftEncoder.GetDistance()},
units::meter_t{m_frontRightEncoder.GetDistance()},
units::meter_t{m_backLeftEncoder.GetDistance()},
units::meter_t{m_backRightEncoder.GetDistance()}};
wpi::math::MecanumDriveWheelPositions Drivetrain::GetCurrentDistances() const {
return {wpi::units::meter_t{m_frontLeftEncoder.GetDistance()},
wpi::units::meter_t{m_frontRightEncoder.GetDistance()},
wpi::units::meter_t{m_backLeftEncoder.GetDistance()},
wpi::units::meter_t{m_backRightEncoder.GetDistance()}};
}
void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) {
std::function<void(units::meters_per_second_t, const frc::Encoder&,
frc::PIDController&, frc::PWMSparkMax&)>
void Drivetrain::SetSpeeds(const wpi::math::MecanumDriveWheelSpeeds& wheelSpeeds) {
std::function<void(wpi::units::meters_per_second_t, const wpi::Encoder&,
wpi::math::PIDController&, wpi::PWMSparkMax&)>
calcAndSetSpeeds = [&m_feedforward = m_feedforward](
units::meters_per_second_t speed,
wpi::units::meters_per_second_t speed,
const auto& encoder, auto& controller,
auto& motor) {
auto feedforward = m_feedforward.Calculate(speed);
double output = controller.Calculate(encoder.GetRate(), speed.value());
motor.SetVoltage(units::volt_t{output} + feedforward);
motor.SetVoltage(wpi::units::volt_t{output} + feedforward);
};
calcAndSetSpeeds(wheelSpeeds.frontLeft, m_frontLeftEncoder,
@@ -44,11 +44,11 @@ void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) {
m_backRightPIDController, m_backRightMotor);
}
void Drivetrain::Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed,
units::radians_per_second_t rot, bool fieldRelative,
units::second_t period) {
frc::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot};
void Drivetrain::Drive(wpi::units::meters_per_second_t xSpeed,
wpi::units::meters_per_second_t ySpeed,
wpi::units::radians_per_second_t rot, bool fieldRelative,
wpi::units::second_t period) {
wpi::math::ChassisSpeeds chassisSpeeds{xSpeed, ySpeed, rot};
if (fieldRelative) {
chassisSpeeds = chassisSpeeds.ToRobotRelative(
m_poseEstimator.GetEstimatedPosition().Rotation());
@@ -68,5 +68,5 @@ void Drivetrain::UpdateOdometry() {
m_poseEstimator.AddVisionMeasurement(
ExampleGlobalMeasurementSensor::GetEstimatedGlobalPose(
m_poseEstimator.GetEstimatedPosition()),
frc::Timer::GetTimestamp() - 0.3_s);
wpi::Timer::GetTimestamp() - 0.3_s);
}

View File

@@ -8,7 +8,7 @@
#include "Drivetrain.hpp"
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
void AutonomousPeriodic() override {
DriveWithJoystick(false);
@@ -18,14 +18,14 @@ class Robot : public frc::TimedRobot {
void TeleopPeriodic() override { DriveWithJoystick(true); }
private:
frc::XboxController m_controller{0};
wpi::XboxController m_controller{0};
Drivetrain m_mecanum;
// Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
// to 1.
frc::SlewRateLimiter<units::scalar> m_xspeedLimiter{3 / 1_s};
frc::SlewRateLimiter<units::scalar> m_yspeedLimiter{3 / 1_s};
frc::SlewRateLimiter<units::scalar> m_rotLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_xspeedLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_yspeedLimiter{3 / 1_s};
wpi::math::SlewRateLimiter<wpi::units::scalar> m_rotLimiter{3 / 1_s};
void DriveWithJoystick(bool fieldRelative) {
// Get the x speed. We are inverting this because Xbox controllers return
@@ -52,6 +52,6 @@ class Robot : public frc::TimedRobot {
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -31,52 +31,52 @@ class Drivetrain {
m_backRightMotor.SetInverted(true);
}
frc::MecanumDriveWheelSpeeds GetCurrentState() const;
frc::MecanumDriveWheelPositions GetCurrentDistances() const;
void SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds);
void Drive(units::meters_per_second_t xSpeed,
units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
bool fieldRelative, units::second_t period);
wpi::math::MecanumDriveWheelSpeeds GetCurrentState() const;
wpi::math::MecanumDriveWheelPositions GetCurrentDistances() const;
void SetSpeeds(const wpi::math::MecanumDriveWheelSpeeds& wheelSpeeds);
void Drive(wpi::units::meters_per_second_t xSpeed,
wpi::units::meters_per_second_t ySpeed, wpi::units::radians_per_second_t rot,
bool fieldRelative, wpi::units::second_t period);
void UpdateOdometry();
static constexpr auto kMaxSpeed = 3.0_mps; // 3 meters per second
static constexpr units::radians_per_second_t kMaxAngularSpeed{
static constexpr wpi::units::radians_per_second_t kMaxAngularSpeed{
std::numbers::pi}; // 1/2 rotation per second
private:
frc::PWMSparkMax m_frontLeftMotor{1};
frc::PWMSparkMax m_frontRightMotor{2};
frc::PWMSparkMax m_backLeftMotor{3};
frc::PWMSparkMax m_backRightMotor{4};
wpi::PWMSparkMax m_frontLeftMotor{1};
wpi::PWMSparkMax m_frontRightMotor{2};
wpi::PWMSparkMax m_backLeftMotor{3};
wpi::PWMSparkMax m_backRightMotor{4};
frc::Encoder m_frontLeftEncoder{0, 1};
frc::Encoder m_frontRightEncoder{2, 3};
frc::Encoder m_backLeftEncoder{4, 5};
frc::Encoder m_backRightEncoder{6, 7};
wpi::Encoder m_frontLeftEncoder{0, 1};
wpi::Encoder m_frontRightEncoder{2, 3};
wpi::Encoder m_backLeftEncoder{4, 5};
wpi::Encoder m_backRightEncoder{6, 7};
frc::Translation2d m_frontLeftLocation{0.381_m, 0.381_m};
frc::Translation2d m_frontRightLocation{0.381_m, -0.381_m};
frc::Translation2d m_backLeftLocation{-0.381_m, 0.381_m};
frc::Translation2d m_backRightLocation{-0.381_m, -0.381_m};
wpi::math::Translation2d m_frontLeftLocation{0.381_m, 0.381_m};
wpi::math::Translation2d m_frontRightLocation{0.381_m, -0.381_m};
wpi::math::Translation2d m_backLeftLocation{-0.381_m, 0.381_m};
wpi::math::Translation2d m_backRightLocation{-0.381_m, -0.381_m};
frc::PIDController m_frontLeftPIDController{1.0, 0.0, 0.0};
frc::PIDController m_frontRightPIDController{1.0, 0.0, 0.0};
frc::PIDController m_backLeftPIDController{1.0, 0.0, 0.0};
frc::PIDController m_backRightPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController m_frontLeftPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController m_frontRightPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController m_backLeftPIDController{1.0, 0.0, 0.0};
wpi::math::PIDController m_backRightPIDController{1.0, 0.0, 0.0};
frc::OnboardIMU m_imu{frc::OnboardIMU::kFlat};
wpi::OnboardIMU m_imu{wpi::OnboardIMU::kFlat};
frc::MecanumDriveKinematics m_kinematics{
wpi::math::MecanumDriveKinematics m_kinematics{
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
m_backRightLocation};
// Gains are for example purposes only - must be determined for your own
// robot!
frc::SimpleMotorFeedforward<units::meters> m_feedforward{1_V, 3_V / 1_mps};
wpi::math::SimpleMotorFeedforward<wpi::units::meters> m_feedforward{1_V, 3_V / 1_mps};
// Gains are for example purposes only - must be determined for your own
// robot!
frc::MecanumDrivePoseEstimator m_poseEstimator{
wpi::math::MecanumDrivePoseEstimator m_poseEstimator{
m_kinematics, m_imu.GetRotation2d(), GetCurrentDistances(),
frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.1, 0.1, 0.1}};
wpi::math::Pose2d{}, {0.1, 0.1, 0.1}, {0.1, 0.1, 0.1}};
};

View File

@@ -13,12 +13,12 @@
*/
class ExampleGlobalMeasurementSensor {
public:
static frc::Pose2d GetEstimatedGlobalPose(
const frc::Pose2d& estimatedRobotPose) {
auto randVec = frc::MakeWhiteNoiseVector(0.1, 0.1, 0.1);
return frc::Pose2d{estimatedRobotPose.X() + units::meter_t{randVec(0)},
estimatedRobotPose.Y() + units::meter_t{randVec(1)},
static wpi::math::Pose2d GetEstimatedGlobalPose(
const wpi::math::Pose2d& estimatedRobotPose) {
auto randVec = wpi::math::MakeWhiteNoiseVector(0.1, 0.1, 0.1);
return wpi::math::Pose2d{estimatedRobotPose.X() + wpi::units::meter_t{randVec(0)},
estimatedRobotPose.Y() + wpi::units::meter_t{randVec(1)},
estimatedRobotPose.Rotation() +
frc::Rotation2d{units::radian_t{randVec(2)}}};
wpi::math::Rotation2d{wpi::units::radian_t{randVec(2)}}};
}
};

View File

@@ -25,7 +25,7 @@
* the container out of scope - the appended nodes will be recursively
* destructed!
*/
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
static constexpr double kMetersPerPulse = 0.01;
static constexpr double kElevatorMinimumLength = 0.5;
@@ -34,14 +34,14 @@ class Robot : public frc::TimedRobot {
m_elevatorEncoder.SetDistancePerPulse(kMetersPerPulse);
// publish to dashboard
frc::SmartDashboard::PutData("Mech2d", &m_mech);
wpi::SmartDashboard::PutData("Mech2d", &m_mech);
}
void RobotPeriodic() override {
// update the dashboard mechanism's state
m_elevator->SetLength(kElevatorMinimumLength +
m_elevatorEncoder.GetDistance());
m_wrist->SetAngle(units::degree_t{m_wristPotentiometer.Get()});
m_wrist->SetAngle(wpi::units::degree_t{m_wristPotentiometer.Get()});
}
void TeleopPeriodic() override {
@@ -50,27 +50,27 @@ class Robot : public frc::TimedRobot {
}
private:
frc::PWMSparkMax m_elevatorMotor{0};
frc::PWMSparkMax m_wristMotor{1};
frc::Encoder m_elevatorEncoder{0, 1};
frc::AnalogPotentiometer m_wristPotentiometer{1, 90};
frc::Joystick m_joystick{0};
wpi::PWMSparkMax m_elevatorMotor{0};
wpi::PWMSparkMax m_wristMotor{1};
wpi::Encoder m_elevatorEncoder{0, 1};
wpi::AnalogPotentiometer m_wristPotentiometer{1, 90};
wpi::Joystick m_joystick{0};
// the main mechanism object
frc::Mechanism2d m_mech{3, 3};
wpi::Mechanism2d m_mech{3, 3};
// the mechanism root node
frc::MechanismRoot2d* m_root = m_mech.GetRoot("climber", 2, 0);
wpi::MechanismRoot2d* m_root = m_mech.GetRoot("climber", 2, 0);
// MechanismLigament2d objects represent each "section"/"stage" of the
// mechanism, and are based off the root node or another ligament object
frc::MechanismLigament2d* m_elevator =
m_root->Append<frc::MechanismLigament2d>("elevator", 1, 90_deg);
frc::MechanismLigament2d* m_wrist =
m_elevator->Append<frc::MechanismLigament2d>(
"wrist", 0.5, 90_deg, 6, frc::Color8Bit{frc::Color::kPurple});
wpi::MechanismLigament2d* m_elevator =
m_root->Append<wpi::MechanismLigament2d>("elevator", 1, 90_deg);
wpi::MechanismLigament2d* m_wrist =
m_elevator->Append<wpi::MechanismLigament2d>(
"wrist", 0.5, 90_deg, 6, wpi::Color8Bit{wpi::Color::kPurple});
};
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -21,7 +21,7 @@
* In addition, the encoder value of an encoder connected to ports 0 and 1 is
* consistently sent to the Dashboard.
*/
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
void TeleopPeriodic() override { m_motor.Set(m_stick.GetY()); }
@@ -30,7 +30,7 @@ class Robot : public frc::TimedRobot {
* robot mode.
*/
void RobotPeriodic() override {
frc::SmartDashboard::PutNumber("Encoder", m_encoder.GetDistance());
wpi::SmartDashboard::PutNumber("Encoder", m_encoder.GetDistance());
}
Robot() {
@@ -40,13 +40,13 @@ class Robot : public frc::TimedRobot {
}
private:
frc::Joystick m_stick{0};
frc::PWMSparkMax m_motor{0};
frc::Encoder m_encoder{0, 1};
wpi::Joystick m_stick{0};
wpi::PWMSparkMax m_motor{0};
wpi::Encoder m_encoder{0, 1};
};
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -12,7 +12,7 @@ void Robot::TeleopInit() {
void Robot::TeleopPeriodic() {
// Read from the sensor
units::meter_t position = units::meter_t{m_potentiometer.Get()};
wpi::units::meter_t position = wpi::units::meter_t{m_potentiometer.Get()};
// Run the PID Controller
double pidOut = m_pidController.Calculate(position.value());
@@ -31,6 +31,6 @@ void Robot::TeleopPeriodic() {
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -18,7 +18,7 @@
* PID controller to reach and maintain position setpoints on an elevator
* mechanism.
*/
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
void TeleopInit() override;
void TeleopPeriodic() override;
@@ -28,10 +28,10 @@ class Robot : public frc::TimedRobot {
static constexpr int kJoystickChannel = 3;
// The elevator can move 1.5 meters from top to bottom
static constexpr units::meter_t kFullHeight = 1.5_m;
static constexpr wpi::units::meter_t kFullHeight = 1.5_m;
// Bottom, middle, and top elevator setpoints
static constexpr std::array<units::meter_t, 3> kSetpoints = {
static constexpr std::array<wpi::units::meter_t, 3> kSetpoints = {
{0.2_m, 0.8_m, 1.4_m}};
private:
@@ -45,11 +45,11 @@ class Robot : public frc::TimedRobot {
static constexpr double kD = 0.25;
// Scaling is handled internally
frc::AnalogPotentiometer m_potentiometer{kPotChannel, kFullHeight.value()};
wpi::AnalogPotentiometer m_potentiometer{kPotChannel, kFullHeight.value()};
frc::PWMSparkMax m_elevatorMotor{kMotorChannel};
frc::PIDController m_pidController{kP, kI, kD};
frc::Joystick m_joystick{kJoystickChannel};
wpi::PWMSparkMax m_elevatorMotor{kMotorChannel};
wpi::math::PIDController m_pidController{kP, kI, kD};
wpi::Joystick m_joystick{kJoystickChannel};
size_t m_index;
};

View File

@@ -13,11 +13,11 @@
* the easiest way to get camera images to the dashboard. Just add this to the
* robot class constructor.
*/
class Robot : public frc::TimedRobot {
class Robot : public wpi::TimedRobot {
public:
Robot() {
#if defined(__linux__) || defined(_WIN32)
frc::CameraServer::StartAutomaticCapture();
wpi::CameraServer::StartAutomaticCapture();
#else
std::fputs("Vision only available on Linux or Windows.\n", stderr);
std::fflush(stderr);
@@ -27,6 +27,6 @@ class Robot : public frc::TimedRobot {
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -32,7 +32,7 @@ void RapidReactCommandBot::ConfigureBindings() {
// Fire the shooter with the A button
m_driverController.A().OnTrue(
frc2::cmd::Parallel(
wpi::cmd::cmd::Parallel(
m_shooter.ShootCommand(ShooterConstants::kShooterTarget),
m_storage.RunCommand())
// Since we composed this inline we should give it a name
@@ -43,7 +43,7 @@ void RapidReactCommandBot::ConfigureBindings() {
m_pneumatics.DisableCompressorCommand());
}
frc2::CommandPtr RapidReactCommandBot::GetAutonomousCommand() {
wpi::cmd::CommandPtr RapidReactCommandBot::GetAutonomousCommand() {
return m_drive
.DriveDistanceCommand(AutoConstants::kDriveDistance,
AutoConstants::kDriveSpeed)

View File

@@ -15,7 +15,7 @@ void Robot::RobotPeriodic() {
// finished or interrupted commands, and running subsystem Periodic() methods.
// This must be called from the robot's periodic block in order for anything
// in the Command-based framework to work.
frc2::CommandScheduler::GetInstance().Run();
wpi::cmd::CommandScheduler::GetInstance().Run();
}
void Robot::DisabledInit() {}
@@ -26,7 +26,7 @@ void Robot::AutonomousInit() {
m_autonomousCommand = m_robot.GetAutonomousCommand();
if (m_autonomousCommand) {
frc2::CommandScheduler::GetInstance().Schedule(m_autonomousCommand.value());
wpi::cmd::CommandScheduler::GetInstance().Schedule(m_autonomousCommand.value());
}
}
@@ -46,13 +46,13 @@ void Robot::TeleopPeriodic() {}
void Robot::TestInit() {
// Cancels all running commands at the start of test mode.
frc2::CommandScheduler::GetInstance().CancelAll();
wpi::cmd::CommandScheduler::GetInstance().CancelAll();
}
void Robot::TestPeriodic() {}
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
return wpi::StartRobot<Robot>();
}
#endif

Some files were not shown because too many files have changed in this diff Show More