[examples] Clean up examples (#8674)

Move various "examples" into snippets. Several examples that were less
than a full mechanism or robot were moved to snippets. `arcadedrive` and
`tankdrive` were removed in favor of their Gamepad variants. `hidrumble`
was removed due to being too simple. `potentiometerpid` was removed
because of low utility. `gyromecanum` replaced `mecanumdrive` for
deduplication and because very few teams run holonomic drivetrains
without gyros.
This commit is contained in:
Gold856
2026-03-14 17:13:45 -04:00
committed by GitHub
parent 62ce8944aa
commit f1adce4cf7
78 changed files with 569 additions and 1665 deletions

View File

@@ -0,0 +1,24 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "Robot.hpp"
Robot::Robot() {
// Default to a length of 60, start empty output
m_led.SetLength(kLength);
m_led.SetData(m_ledBuffer);
}
void Robot::RobotPeriodic() {
// Run the rainbow pattern and apply it to the buffer
m_scrollingRainbow.ApplyTo(m_ledBuffer);
// Set the LEDs
m_led.SetData(m_ledBuffer);
}
#ifndef RUNNING_WPILIB_TESTS
int main() {
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -0,0 +1,37 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <array>
#include "wpi/framework/TimedRobot.hpp"
#include "wpi/hardware/led/AddressableLED.hpp"
#include "wpi/hardware/led/LEDPattern.hpp"
class Robot : public wpi::TimedRobot {
public:
Robot();
void RobotPeriodic() override;
private:
static constexpr int kLength = 60;
// SmartIO port 1
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
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
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 velocity of 1 meter per second.
wpi::LEDPattern m_scrollingRainbow =
m_rainbow.ScrollAtAbsoluteVelocity(1_mps, kLedSpacing);
};

View File

@@ -0,0 +1,173 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <cstdio>
#include <span>
#include <string>
#include <thread>
#include <vector>
#include <fmt/format.h>
#include <opencv2/core/core.hpp>
#include <opencv2/core/types.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include "wpi/apriltag/AprilTagDetection.hpp"
#include "wpi/apriltag/AprilTagDetector.hpp"
#include "wpi/apriltag/AprilTagPoseEstimator.hpp"
#include "wpi/cameraserver/CameraServer.hpp"
#include "wpi/framework/TimedRobot.hpp"
#include "wpi/math/geometry/Transform3d.hpp"
#include "wpi/nt/IntegerArrayTopic.hpp"
#include "wpi/nt/NetworkTableInstance.hpp"
#include "wpi/units/angle.hpp"
#include "wpi/units/length.hpp"
/**
* This is a demo program showing the detection of AprilTags.
* The image is acquired from the USB camera, then any detected AprilTags
* are marked up on the image and sent to the dashboard.
*
* Be aware that the performance on this is much worse than a coprocessor
* solution!
*/
class Robot : public wpi::TimedRobot {
public:
Robot() {
// We need to run our vision program in a separate thread. If not, our robot
// program will not run.
#if defined(__linux__) || defined(_WIN32)
std::thread visionThread(VisionThread);
visionThread.detach();
#else
std::fputs("Vision only available on Linux or Windows.\n", stderr);
std::fflush(stderr);
#endif
}
#if defined(__linux__) || defined(_WIN32)
private:
static void VisionThread() {
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
// RoboRIO 2
detector.AddFamily("tag36h11", 1);
// Set up Pose Estimator - parameters are for a Microsoft Lifecam HD-3000
// (https://www.chiefdelphi.com/t/wpilib-apriltagdetector-sample-code/421411/21)
wpi::apriltag::AprilTagPoseEstimator::Config poseEstConfig = {
.tagSize = 6.5_in,
.fx = 699.3778103158814,
.fy = 677.7161226393544,
.cx = 345.6059345433618,
.cy = 207.12741326228522};
wpi::apriltag::AprilTagPoseEstimator estimator(poseEstConfig);
// Get the USB camera from CameraServer
wpi::cs::UsbCamera camera = wpi::CameraServer::StartAutomaticCapture();
// Set the resolution
camera.SetResolution(640, 480);
// Get a CvSink. This will capture Mats from the Camera
wpi::cs::CvSink cvSink = wpi::CameraServer::GetVideo();
// Setup a CvSource. This will send images back to the Dashboard
wpi::cs::CvSource outputStream =
wpi::CameraServer::PutVideo("Detected", 640, 480);
// Mats are very memory expensive. Lets reuse this Mat.
cv::Mat mat;
cv::Mat grayMat;
// Instantiate once
std::vector<int64_t> tags;
cv::Scalar outlineColor{0, 255, 0};
cv::Scalar crossColor{0, 0, 255};
// We'll output to NT
auto tagsTable =
wpi::nt::NetworkTableInstance::GetDefault().GetTable("apriltags");
auto pubTags = tagsTable->GetIntegerArrayTopic("tags").Publish();
while (true) {
// Tell the CvSink to grab a frame from the camera and
// put it in the source mat. If there is an error notify the
// output.
if (cvSink.GrabFrame(mat) == 0) {
// Send the output the error.
outputStream.NotifyError(cvSink.GetError());
// skip the rest of the current iteration
continue;
}
cv::cvtColor(mat, grayMat, cv::COLOR_BGR2GRAY);
cv::Size g_size = grayMat.size();
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 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 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 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);
line(mat, cv::Point(c.x, c.y - ll), cv::Point(c.x, c.y + ll),
crossColor, 2);
// identify the tag
putText(mat, std::to_string(detection->GetId()),
cv::Point(c.x + ll, c.y), cv::FONT_HERSHEY_SIMPLEX, 1,
crossColor, 3);
// determine pose
wpi::math::Transform3d pose = estimator.Estimate(*detection);
// put pose into NT
wpi::math::Rotation3d rotation = pose.Rotation();
tagsTable->GetEntry(fmt::format("pose_{}", detection->GetId()))
.SetDoubleArray(
{{ pose.X().value(),
pose.Y().value(),
pose.Z().value(),
rotation.X().value(),
rotation.Y().value(),
rotation.Z().value() }});
}
// put list of tags onto NT
pubTags.Set(tags);
// Give the output stream a new image to display
outputStream.PutFrame(mat);
}
}
#endif
};
#ifndef RUNNING_WPILIB_TESTS
int main() {
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -0,0 +1,61 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/framework/TimedRobot.hpp"
#include "wpi/hardware/power/PowerDistribution.hpp"
#include "wpi/smartdashboard/SmartDashboard.hpp"
/**
* This is a sample program showing how to retrieve information from the Power
* Distribution Panel via CAN. The information will be displayed under variables
* through the SmartDashboard.
*/
class Robot : public wpi::TimedRobot {
public:
Robot() {
// Put the PDP itself to the dashboard
wpi::SmartDashboard::PutData("PDP", &m_pdp);
}
void RobotPeriodic() override {
// Get the current going through channel 7, in Amperes.
// 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);
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();
wpi::SmartDashboard::PutNumber("Voltage", voltage);
// Retrieves the temperature of the PDP, in degrees Celsius.
double temperatureCelsius = m_pdp.GetTemperature();
wpi::SmartDashboard::PutNumber("Temperature", temperatureCelsius);
// Get the total current of all channels.
double totalCurrent = m_pdp.GetTotalCurrent();
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();
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();
wpi::SmartDashboard::PutNumber("Total Energy", totalEnergy);
}
private:
// Object for dealing with the Power Distribution Panel (PDP).
wpi::PowerDistribution m_pdp{0};
};
#ifndef RUNNING_WPILIB_TESTS
int main() {
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -0,0 +1,29 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "Robot.hpp"
#include "wpi/driverstation/DriverStation.hpp"
void Robot::RobotPeriodic() {
// pull alliance port high if on red alliance, pull low if on blue alliance
m_allianceOutput.Set(wpi::DriverStation::GetAlliance() ==
wpi::DriverStation::kRed);
// pull enabled port high if enabled, low if disabled
m_enabledOutput.Set(wpi::DriverStation::IsEnabled());
// pull auto port high if in autonomous, low if in teleop (or disabled)
m_autonomousOutput.Set(wpi::DriverStation::IsAutonomous());
// pull alert port high if match time remaining is between 30 and 25 seconds
auto matchTime = wpi::DriverStation::GetMatchTime();
m_alertOutput.Set(matchTime <= 30_s && matchTime >= 25_s);
}
#ifndef RUNNING_WPILIB_TESTS
int main() {
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -0,0 +1,31 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <array>
#include "wpi/framework/TimedRobot.hpp"
#include "wpi/hardware/discrete/DigitalOutput.hpp"
/**
* 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 wpi::TimedRobot {
public:
// define ports for communication with light controller
static constexpr int kAlliancePort = 0;
static constexpr int kEnabledPort = 1;
static constexpr int kAutonomousPort = 2;
static constexpr int kAlertPort = 3;
void RobotPeriodic() override;
private:
wpi::DigitalOutput m_allianceOutput{kAlliancePort};
wpi::DigitalOutput m_enabledOutput{kEnabledPort};
wpi::DigitalOutput m_autonomousOutput{kAutonomousPort};
wpi::DigitalOutput m_alertOutput{kAlertPort};
};

View File

@@ -0,0 +1,33 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/framework/TimedRobot.hpp"
#include "wpi/hardware/discrete/DigitalInput.hpp"
#include "wpi/hardware/rotation/DutyCycle.hpp"
#include "wpi/smartdashboard/SmartDashboard.hpp"
class Robot : public wpi::TimedRobot {
wpi::DutyCycle m_dutyCycle{0}; // Duty cycle input
public:
Robot() {}
void RobotPeriodic() override {
// Duty Cycle Frequency in Hz
auto frequency = m_dutyCycle.GetFrequency();
// Output of duty cycle, ranging from 0 to 1
// 1 is fully on, 0 is fully off
auto output = m_dutyCycle.GetOutput();
wpi::SmartDashboard::PutNumber("Frequency", frequency.value());
wpi::SmartDashboard::PutNumber("Duty Cycle", output);
}
};
#ifndef RUNNING_WPILIB_TESTS
int main() {
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -0,0 +1,104 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/driverstation/Joystick.hpp"
#include "wpi/event/BooleanEvent.hpp"
#include "wpi/event/EventLoop.hpp"
#include "wpi/framework/TimedRobot.hpp"
#include "wpi/hardware/motor/PWMSparkMax.hpp"
#include "wpi/hardware/rotation/Encoder.hpp"
#include "wpi/math/controller/PIDController.hpp"
#include "wpi/math/controller/SimpleMotorFeedforward.hpp"
#include "wpi/units/angular_velocity.hpp"
#include "wpi/units/length.hpp"
#include "wpi/units/time.hpp"
#include "wpi/units/voltage.hpp"
static const auto SHOT_VELOCITY = 200_rpm;
static const auto TOLERANCE = 8_rpm;
class Robot : public wpi::TimedRobot {
public:
Robot() {
m_controller.SetTolerance(TOLERANCE.value());
wpi::BooleanEvent isBallAtKicker{&m_loop, [] {
return false;
// return kickerSensor.GetRange() <
// KICKER_THRESHOLD;
}};
wpi::BooleanEvent intakeButton{
&m_loop, [&joystick = m_joystick] { return joystick.GetRawButton(2); }};
// if the thumb button is held
(intakeButton
// and there is not a ball at the kicker
&& !isBallAtKicker)
// activate the intake
.IfHigh([&intake = m_intake] { intake.SetDutyCycle(0.5); });
// if the thumb button is not held
(!intakeButton
// or there is a ball in the kicker
|| isBallAtKicker)
// stop the intake
.IfHigh([&intake = m_intake] { intake.SetDutyCycle(0.0); });
wpi::BooleanEvent shootTrigger{
&m_loop, [&joystick = m_joystick] { return joystick.GetTrigger(); }};
// if the trigger is held
shootTrigger
// accelerate the shooter wheel
.IfHigh([&shooter = m_shooter, &controller = m_controller, &ff = m_ff,
&encoder = m_shooterEncoder] {
shooter.SetVoltage(
wpi::units::volt_t{controller.Calculate(encoder.GetRate(),
SHOT_VELOCITY.value())} +
ff.Calculate(wpi::units::radians_per_second_t{SHOT_VELOCITY}));
});
// if not, stop
(!shootTrigger).IfHigh([&shooter = m_shooter] {
shooter.SetDutyCycle(0.0);
});
wpi::BooleanEvent atTargetVelocity =
wpi::BooleanEvent(
&m_loop,
[&controller = m_controller] { return controller.AtSetpoint(); })
// debounce for more stability
.Debounce(0.2_s);
// if we're at the target velocity, kick the ball into the shooter wheel
atTargetVelocity.IfHigh([&kicker = m_kicker] { kicker.SetDutyCycle(0.7); });
// when we stop being at the target velocity, it means the ball was shot
atTargetVelocity
.Falling()
// so stop the kicker
.IfHigh([&kicker = m_kicker] { kicker.SetDutyCycle(0.0); });
}
void RobotPeriodic() override { m_loop.Poll(); }
private:
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};
wpi::PWMSparkMax m_kicker{1};
wpi::PWMSparkMax m_intake{3};
wpi::EventLoop m_loop{};
wpi::Joystick m_joystick{0};
};
#ifndef RUNNING_WPILIB_TESTS
int main() {
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -0,0 +1,110 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/driverstation/Joystick.hpp"
#include "wpi/framework/TimedRobot.hpp"
#include "wpi/hardware/motor/PWMSparkMax.hpp"
#include "wpi/hardware/rotation/Encoder.hpp"
#include "wpi/math/controller/BangBangController.hpp"
#include "wpi/math/controller/SimpleMotorFeedforward.hpp"
#include "wpi/math/system/Models.hpp"
#include "wpi/simulation/EncoderSim.hpp"
#include "wpi/simulation/FlywheelSim.hpp"
#include "wpi/smartdashboard/SmartDashboard.hpp"
#include "wpi/units/moment_of_inertia.hpp"
/**
* This is a sample program to demonstrate the use of a
* wpi::math::BangBangController with a flywheel to control velocity.
*/
class Robot : public wpi::TimedRobot {
public:
/**
* Controls flywheel to a set velocity (RPM) controlled by a joystick.
*/
void TeleopPeriodic() override {
// Scale setpoint value between 0 and maxSetpointValue
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
wpi::units::volt_t bangOutput =
m_bangBangController.Calculate(m_encoder.GetRate(), setpoint.value()) *
12_V;
// Controls a motor with the output of the BangBang controller and a
// feedforward. The feedforward is reduced slightly to avoid overspeeding
// the shooter.
m_flywheelMotor.SetVoltage(bangOutput +
0.9 * m_feedforward.Calculate(setpoint));
}
Robot() {
// Add bang-bang controller to SmartDashboard and networktables.
wpi::SmartDashboard::PutData("BangBangController", &m_bangBangController);
}
/**
* Update our simulation. This should be run every robot loop in simulation.
*/
void SimulationPeriodic() override {
// To update our simulation, we set motor voltage inputs, update the
// simulation, and write the simulated velocities to our simulated encoder
m_flywheelSim.SetInputVoltage(
m_flywheelMotor.GetDutyCycle() *
wpi::units::volt_t{wpi::RobotController::GetInputVoltage()});
m_flywheelSim.Update(20_ms);
m_encoderSim.SetRate(m_flywheelSim.GetAngularVelocity().value());
}
private:
static constexpr int kMotorPort = 0;
static constexpr int kEncoderAChannel = 0;
static constexpr int kEncoderBChannel = 1;
// Max setpoint for joystick control
static constexpr wpi::units::radians_per_second_t kMaxSetpointValue =
6000_rpm;
// Joystick to control setpoint
wpi::Joystick m_joystick{0};
wpi::PWMSparkMax m_flywheelMotor{kMotorPort};
wpi::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
wpi::math::BangBangController m_bangBangController;
// Gains are for example purposes only - must be determined for your own
// robot!
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;
wpi::math::SimpleMotorFeedforward<wpi::units::radians> m_feedforward{
kFlywheelKs, kFlywheelKv, kFlywheelKa};
// Simulation classes help us simulate our robot
// Reduction between motors and encoder, as output over input. If the flywheel
// spins slower than the motors, this number should be greater than one.
static constexpr double kFlywheelGearing = 1.0;
// 1/2 MR²
static constexpr wpi::units::kilogram_square_meter_t
kFlywheelMomentOfInertia = 0.5 * 1.5_lb * 4_in * 4_in;
wpi::math::DCMotor m_gearbox = wpi::math::DCMotor::NEO(1);
wpi::math::LinearSystem<1, 1, 1> m_plant{
wpi::math::Models::FlywheelFromPhysicalConstants(
m_gearbox, kFlywheelMomentOfInertia, kFlywheelGearing)};
wpi::sim::FlywheelSim m_flywheelSim{m_plant, m_gearbox};
wpi::sim::EncoderSim m_encoderSim{m_encoder};
};
#ifndef RUNNING_WPILIB_TESTS
int main() {
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -0,0 +1,70 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <opencv2/core/core.hpp>
#include <opencv2/core/types.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include "wpi/cameraserver/CameraServer.hpp"
#include "wpi/cs/HttpCamera.hpp"
#include "wpi/framework/TimedRobot.hpp"
/**
* This is a demo program showing the use of OpenCV to do vision processing. The
* image is acquired from an HTTP camera, then a rectangle is put on the image
* and sent to the dashboard. OpenCV has many methods for different types of
* processing.
*/
class Robot : public wpi::TimedRobot {
public:
Robot() {
// We need to run our vision program in a separate thread. If not, our robot
// program will not run.
std::thread visionThread(VisionThread);
visionThread.detach();
}
private:
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.
wpi::cs::HttpCamera camera{"My Camera",
"http://10.x.y.11/video/stream.mjpg"};
// Start capturing images
wpi::CameraServer::StartAutomaticCapture(camera);
// Set the resolution
camera.SetResolution(640, 480);
// Get a CvSink. This will capture Mats from the Camera
wpi::cs::CvSink cvSink = wpi::CameraServer::GetVideo();
// Setup a CvSource. This will send images back to the Dashboard
wpi::cs::CvSource outputStream =
wpi::CameraServer::PutVideo("Rectangle", 640, 480);
// Mats are very memory expensive. Lets reuse this Mat.
cv::Mat mat;
while (true) {
// Tell the CvSink to grab a frame from the camera and put it in the
// source mat. If there is an error notify the output.
if (cvSink.GrabFrame(mat) == 0) {
// Send the output the error.
outputStream.NotifyError(cvSink.GetError());
// skip the rest of the current iteration
continue;
}
// Put a rectangle on the image
rectangle(mat, cv::Point(100, 100), cv::Point(400, 400),
cv::Scalar(255, 255, 255), 5);
// Give the output stream a new image to display
outputStream.PutFrame(mat);
}
}
};
#ifndef RUNNING_WPILIB_TESTS
int main() {
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -0,0 +1,48 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "Robot.hpp"
#include <string>
#include <fmt/format.h>
#include "wpi/driverstation/DriverStation.hpp"
#include "wpi/system/Timer.hpp"
void Robot::RobotPeriodic() {
// Creates a string to hold current robot state information, including
// alliance, enabled state, operation mode, and match time. The message
// is sent in format "AEM###" where A is the alliance color, (R)ed or
// (B)lue, E is the enabled state, (E)nabled or (D)isabled, M is the
// operation mode, (A)utonomous or (T)eleop, and ### is the zero-padded
// time remaining in the match.
//
// For example, "RET043" would indicate that the robot is on the red
// alliance, enabled in teleop mode, with 43 seconds left in the match.
std::string allianceString = "U";
auto alliance = wpi::DriverStation::GetAlliance();
if (alliance.has_value()) {
if (alliance == wpi::DriverStation::Alliance::kRed) {
allianceString = "R";
} else {
allianceString = "B";
}
}
auto string =
fmt::format("{}{}{}{:03}", allianceString,
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_WPILIB_TESTS
int main() {
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -0,0 +1,25 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <array>
#include "wpi/framework/TimedRobot.hpp"
#include "wpi/hardware/bus/I2C.hpp"
/**
* 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 wpi::TimedRobot {
public:
void RobotPeriodic() override;
static constexpr wpi::I2C::Port kPort = wpi::I2C::Port::kPort0;
private:
static constexpr int deviceAddress = 4;
wpi::I2C arduino{kPort, deviceAddress};
};

View File

@@ -0,0 +1,78 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <cstdio>
#include <thread>
#include <opencv2/core/core.hpp>
#include <opencv2/core/types.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include "wpi/cameraserver/CameraServer.hpp"
#include "wpi/framework/TimedRobot.hpp"
/**
* This is a demo program showing the use of OpenCV to do vision processing. The
* image is acquired from the USB camera, then a rectangle is put on the image
* and sent to the dashboard. OpenCV has many methods for different types of
* processing.
*/
class Robot : public wpi::TimedRobot {
public:
Robot() {
// We need to run our vision program in a separate thread. If not, our robot
// program will not run.
#if defined(__linux__) || defined(_WIN32)
std::thread visionThread(VisionThread);
visionThread.detach();
#else
std::fputs("Vision only available on Linux or Windows.\n", stderr);
std::fflush(stderr);
#endif
}
#if defined(__linux__) || defined(_WIN32)
private:
static void VisionThread() {
// Get the USB camera from CameraServer
wpi::cs::UsbCamera camera = wpi::CameraServer::StartAutomaticCapture();
// Set the resolution
camera.SetResolution(640, 480);
// Get a CvSink. This will capture Mats from the Camera
wpi::cs::CvSink cvSink = wpi::CameraServer::GetVideo();
// Setup a CvSource. This will send images back to the Dashboard
wpi::cs::CvSource outputStream =
wpi::CameraServer::PutVideo("Rectangle", 640, 480);
// Mats are very memory expensive. Lets reuse this Mat.
cv::Mat mat;
while (true) {
// Tell the CvSink to grab a frame from the camera and
// put it
// in the source mat. If there is an error notify the
// output.
if (cvSink.GrabFrame(mat) == 0) {
// Send the output the error.
outputStream.NotifyError(cvSink.GetError());
// skip the rest of the current iteration
continue;
}
// Put a rectangle on the image
rectangle(mat, cv::Point(100, 100), cv::Point(400, 400),
cv::Scalar(255, 255, 255), 5);
// Give the output stream a new image to display
outputStream.PutFrame(mat);
}
}
#endif
};
#ifndef RUNNING_WPILIB_TESTS
int main() {
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -0,0 +1,52 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <numbers>
#include "wpi/driverstation/Joystick.hpp"
#include "wpi/framework/TimedRobot.hpp"
#include "wpi/hardware/motor/PWMSparkMax.hpp"
#include "wpi/hardware/rotation/Encoder.hpp"
#include "wpi/smartdashboard/SmartDashboard.hpp"
/**
* This sample program shows how to control a motor using a joystick. In the
* operator control part of the program, the joystick is read and the value is
* written to the motor.
*
* Joystick analog values range from -1 to 1 and motor controller inputs as
* range from -1 to 1 making it easy to work together.
*
* In addition, the encoder value of an encoder connected to ports 0 and 1 is
* consistently sent to the Dashboard.
*/
class Robot : public wpi::TimedRobot {
public:
void TeleopPeriodic() override { m_motor.SetDutyCycle(m_stick.GetY()); }
/*
* The RobotPeriodic function is called every control packet no matter the
* robot mode.
*/
void RobotPeriodic() override {
wpi::SmartDashboard::PutNumber("Encoder", m_encoder.GetDistance());
}
Robot() {
// Use SetDistancePerPulse to set the multiplier for GetDistance
// This is set up assuming a 6 inch wheel with a 360 CPR encoder.
m_encoder.SetDistancePerPulse((std::numbers::pi * 6) / 360.0);
}
private:
wpi::Joystick m_stick{0};
wpi::PWMSparkMax m_motor{0};
wpi::Encoder m_encoder{0, 1};
};
#ifndef RUNNING_WPILIB_TESTS
int main() {
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -0,0 +1,32 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <cstdio>
#include "wpi/cameraserver/CameraServer.hpp"
#include "wpi/framework/TimedRobot.hpp"
/**
* Uses the CameraServer class to automatically capture video from a USB webcam
* and send it to the dashboard without doing any vision processing. This is
* the easiest way to get camera images to the dashboard. Just add this to
* the robot class constructor.
*/
class Robot : public wpi::TimedRobot {
public:
Robot() {
#if defined(__linux__) || defined(_WIN32)
wpi::CameraServer::StartAutomaticCapture();
#else
std::fputs("Vision only available on Linux or Windows.\n", stderr);
std::fflush(stderr);
#endif
}
};
#ifndef RUNNING_WPILIB_TESTS
int main() {
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -0,0 +1,72 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "Robot.hpp"
#include "wpi/commands2/CommandScheduler.hpp"
#include "wpi/smartdashboard/SmartDashboard.hpp"
Robot::Robot() {}
/**
* This function is called every 20 ms, no matter the mode. Use
* this for items like diagnostics that you want to run during disabled,
* autonomous, teleoperated and test.
*
* <p> This runs after the mode specific periodic functions, but before
* LiveWindow and SmartDashboard integrated updating.
*/
void Robot::RobotPeriodic() {
wpi::cmd::CommandScheduler::GetInstance().Run();
}
/**
* This function is called once each time the robot enters Disabled mode. You
* can use it to reset any subsystem information you want to clear when the
* robot is disabled.
*/
void Robot::DisabledInit() {}
void Robot::DisabledPeriodic() {}
/**
* This autonomous runs the autonomous command selected by your {@link
* RobotContainer} class.
*/
void Robot::AutonomousInit() {
m_autonomousCommand = m_container.GetAutonomousCommand();
if (m_autonomousCommand != nullptr) {
wpi::cmd::CommandScheduler::GetInstance().Schedule(m_autonomousCommand);
}
}
void Robot::AutonomousPeriodic() {}
void Robot::TeleopInit() {
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
if (m_autonomousCommand != nullptr) {
m_autonomousCommand->Cancel();
m_autonomousCommand = nullptr;
}
}
/**
* This function is called periodically during operator control.
*/
void Robot::TeleopPeriodic() {}
/**
* This function is called periodically during test mode.
*/
void Robot::TestPeriodic() {}
#ifndef RUNNING_WPILIB_TESTS
int main() {
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -0,0 +1,29 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "RobotContainer.hpp"
#include "wpi/smartdashboard/SmartDashboard.hpp"
RobotContainer::RobotContainer() {
// Initialize all of your commands and subsystems here
m_chooser.SetDefaultOption("ONE", CommandSelector::ONE);
m_chooser.AddOption("TWO", CommandSelector::TWO);
m_chooser.AddOption("THREE", CommandSelector::THREE);
wpi::SmartDashboard::PutData("Auto Chooser", &m_chooser);
// Configure the button bindings
ConfigureButtonBindings();
}
void RobotContainer::ConfigureButtonBindings() {
// Configure your button bindings here
}
wpi::cmd::Command* RobotContainer::GetAutonomousCommand() {
// Run the select command in autonomous
return m_exampleSelectCommand.get();
}

View File

@@ -0,0 +1,19 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
/**
* The Constants header provides a convenient place for teams to hold robot-wide
* numerical or boolean constants. This should not be used for any other
* purpose.
*
* It is generally a good idea to place constants into subsystem- or
* command-specific namespaces within this header, which can then be used where
* they are needed.
*/
namespace OIConstants {
inline constexpr int kDriverControllerPort = 0;
} // namespace OIConstants

View File

@@ -0,0 +1,29 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "RobotContainer.hpp"
#include "wpi/commands2/Command.hpp"
#include "wpi/framework/TimedRobot.hpp"
class Robot : public wpi::TimedRobot {
public:
Robot();
void RobotPeriodic() override;
void DisabledInit() override;
void DisabledPeriodic() override;
void AutonomousInit() override;
void AutonomousPeriodic() override;
void TeleopInit() override;
void TeleopPeriodic() override;
void TestPeriodic() override;
private:
// Have it null by default so that if testing teleop it
// doesn't have undefined behavior and potentially crash.
wpi::cmd::Command* m_autonomousCommand = nullptr;
RobotContainer m_container;
};

View File

@@ -0,0 +1,48 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "wpi/commands2/Command.hpp"
#include "wpi/commands2/CommandPtr.hpp"
#include "wpi/commands2/Commands.hpp"
#include "wpi/smartdashboard/SendableChooser.hpp"
/**
* This class is where the bulk of the robot should be declared. Since
* Command-based is a "declarative" paradigm, very little robot logic should
* actually be handled in the {@link Robot} periodic methods (other than the
* scheduler calls). Instead, the structure of the robot (including subsystems,
* commands, and button mappings) should be declared here.
*/
class RobotContainer {
public:
RobotContainer();
wpi::cmd::Command* GetAutonomousCommand();
private:
// The enum used as keys for selecting the command to run.
enum CommandSelector { ONE, TWO, THREE };
// An example of how command selector may be used with SendableChooser
wpi::SendableChooser<CommandSelector> m_chooser;
// The robot's subsystems and commands are defined here...
// An example selectcommand. Will select from the three commands based on the
// value returned by the selector method at runtime. Note that selectcommand
// takes a generic type, so the selector does not have to be an enum; it could
// be any desired type (string, integer, boolean, double...)
wpi::cmd::CommandPtr m_exampleSelectCommand =
wpi::cmd::cmd::Select<CommandSelector>(
[this] { return m_chooser.GetSelected(); },
// Maps selector values to commands
std::pair{ONE, wpi::cmd::cmd::Print("Command one was selected!")},
std::pair{TWO, wpi::cmd::cmd::Print("Command two was selected!")},
std::pair{THREE,
wpi::cmd::cmd::Print("Command three was selected!")});
void ConfigureButtonBindings();
};

View File

@@ -0,0 +1,94 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "Robot.hpp"
#include "wpi/smartdashboard/SmartDashboard.hpp"
#include "wpi/units/pressure.hpp"
Robot::Robot() {
// Publish elements to shuffleboard.
wpi::SmartDashboard::PutData("Single Solenoid", &m_solenoid);
wpi::SmartDashboard::PutData("Double Solenoid", &m_doubleSolenoid);
wpi::SmartDashboard::PutData("Compressor", &m_compressor);
}
void Robot::TeleopPeriodic() {
// Publish some raw data
// Get the pressure (in PSI) from the analog sensor connected to the PH.
// This function is supported only on the PH!
// On a PCM, this function will return 0.
wpi::SmartDashboard::PutNumber("PH Pressure [PSI]",
m_compressor.GetPressure().value());
// Get compressor current draw.
wpi::SmartDashboard::PutNumber("Compressor Current",
m_compressor.GetCurrent().value());
// Get whether the compressor is active.
wpi::SmartDashboard::PutBoolean("Compressor Active",
m_compressor.IsEnabled());
// Get the digital pressure switch connected to the PCM/PH.
// The switch is open when the pressure is over ~120 PSI.
wpi::SmartDashboard::PutBoolean("Pressure Switch",
m_compressor.GetPressureSwitchValue());
/*
* The output of GetRawButton is true/false depending on whether
* the button is pressed; Set takes a boolean for whether
* to retract the solenoid (false) or extend it (true).
*/
m_solenoid.Set(m_stick.GetRawButton(kSolenoidButton));
/*
* GetRawButtonPressed will only return true once per press.
* If a button is pressed, set the solenoid to the respective channel.
*/
if (m_stick.GetRawButtonPressed(kDoubleSolenoidForward)) {
m_doubleSolenoid.Set(wpi::DoubleSolenoid::kForward);
} else if (m_stick.GetRawButtonPressed(kDoubleSolenoidReverse)) {
m_doubleSolenoid.Set(wpi::DoubleSolenoid::kReverse);
}
// On button press, toggle the compressor with the mode selected from the
// dashboard.
if (m_stick.GetRawButtonPressed(kCompressorButton)) {
// Check whether the compressor is currently enabled.
bool isCompressorEnabled = m_compressor.IsEnabled();
if (isCompressorEnabled) {
// Disable closed-loop mode on the compressor.
m_compressor.Disable();
} else {
// Change the if directives to select the closed-loop mode you want to
// use:
#if 0
// Enable closed-loop mode based on the digital pressure switch
// connected to the PCM/PH.
m_compressor.EnableDigital();
#endif
#if 1
// Enable closed-loop mode based on the analog pressure sensor connected
// to the PH. The compressor will run while the pressure reported by the
// sensor is in the specified range ([70 PSI, 120 PSI] in this example).
// Analog mode exists only on the PH! On the PCM, this enables digital
// control.
m_compressor.EnableAnalog(70_psi, 120_psi);
#endif
#if 0
// Enable closed-loop mode based on both the digital pressure switch AND the analog
// pressure sensor connected to the PH.
// The compressor will run while the pressure reported by the analog sensor is in the
// specified range ([70 PSI, 120 PSI] in this example) AND the digital switch reports
// that the system is not full.
// Hybrid mode exists only on the PH! On the PCM, this enables digital control.
m_compressor.EnableHybrid(70_psi, 120_psi);
#endif
}
}
}
#ifndef RUNNING_WPILIB_TESTS
int main() {
return wpi::StartRobot<Robot>();
}
#endif

View File

@@ -0,0 +1,62 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "wpi/driverstation/Joystick.hpp"
#include "wpi/framework/TimedRobot.hpp"
#include "wpi/hardware/pneumatic/Compressor.hpp"
#include "wpi/hardware/pneumatic/DoubleSolenoid.hpp"
#include "wpi/hardware/pneumatic/PneumaticsControlModule.hpp"
#include "wpi/hardware/pneumatic/Solenoid.hpp"
#include "wpi/hardware/rotation/AnalogPotentiometer.hpp"
/**
* This is a sample program showing the use of the solenoid classes during
* operator control.
*
* Three buttons from a joystick will be used to control two solenoids: One
* button to control the position of a single solenoid and the other two buttons
* to control a double solenoid.
*
* Single solenoids can either be on or off, such that the air diverted through
* them goes through either one channel or the other.
*
* Double solenoids have three states: Off, Forward, and Reverse. Forward and
* Reverse divert the air through the two channels and correspond to the on and
* off of a single solenoid, but a double solenoid can also be "off", where both
* channels are diverted to exhaust such that there is no pressure in either
* channel.
*
* Additionally, double solenoids take up two channels on your PCM whereas
* single solenoids only take a single channel.
*/
class Robot : public wpi::TimedRobot {
public:
Robot();
void TeleopPeriodic() override;
private:
wpi::Joystick m_stick{0};
// Solenoid corresponds to a single solenoid.
// In this case, it's connected to channel 0 of a PH with the default CAN
// ID.
wpi::Solenoid m_solenoid{0, wpi::PneumaticsModuleType::REVPH, 0};
// DoubleSolenoid corresponds to a double solenoid.
// In this case, it's connected to channels 1 and 2 of a PH with the default
// CAN ID.
wpi::DoubleSolenoid m_doubleSolenoid{0, wpi::PneumaticsModuleType::REVPH, 1,
2};
// Compressor connected to a PH with a default CAN ID
wpi::Compressor m_compressor{0, wpi::PneumaticsModuleType::REVPH};
static constexpr int kSolenoidButton = 1;
static constexpr int kDoubleSolenoidForward = 2;
static constexpr int kDoubleSolenoidReverse = 3;
static constexpr int kCompressorButton = 4;
};

View File

@@ -156,5 +156,152 @@
],
"foldername": "ProfiledPIDFeedforward",
"gradlebase": "cpp"
},
{
"name": "Addressable LED",
"description": "Display a rainbow pattern on an addressable LED strip.",
"tags": [
"Hardware",
"Basic Robot",
"AddressableLEDs"
],
"foldername": "AddressableLED",
"gradlebase": "cpp"
},
{
"name": "AprilTags Vision",
"description": "On-roboRIO detection of AprilTags using an attached USB camera.",
"tags": [
"Vision",
"AprilTags"
],
"foldername": "AprilTagsVision",
"gradlebase": "cpp"
},
{
"name": "PDP CAN Monitoring",
"description": "Monitor Power Distribution data such as voltage, current, temperature, etc.",
"tags": [
"Hardware",
"PDP",
"SmartDashboard"
],
"foldername": "CANPDP",
"gradlebase": "cpp"
},
{
"name": "Digital Communication Sample",
"description": "Communicates with external devices (such as an Arduino) using the roboRIO's DIO.",
"tags": [
"Hardware",
"Digital Output"
],
"foldername": "DigitalCommunication",
"gradlebase": "cpp",
"hasunittests": true
},
{
"name": "Duty Cycle Input",
"description": "View duty-cycle input.",
"tags": [
"Hardware",
"Duty Cycle",
"SmartDashboard"
],
"foldername": "DutyCycleInput",
"gradlebase": "cpp"
},
{
"name": "EventLoop",
"description": "Manage a ball system using EventLoop and BooleanEvent.",
"tags": [
"Basic Robot",
"Flywheel",
"EventLoop"
],
"foldername": "EventLoop",
"gradlebase": "cpp"
},
{
"name": "Flywheel BangBangController",
"description": "A sample program to demonstrate the use of a BangBangController with a flywheel to control RPM",
"tags": [
"Flywheel",
"Simulation",
"Joystick"
],
"foldername": "FlywheelBangBangController",
"gradlebase": "cpp"
},
{
"name": "HTTP Camera",
"description": "Acquire images from an HTTP network camera and adds some annotation to the image (as you might do for showing operators the result of some image recognition), and sends it to the dashboard for display.",
"tags": [
"Vision"
],
"foldername": "HttpCamera",
"gradlebase": "cpp"
},
{
"name": "I2C Communication",
"description": "Communicate with external devices (such as an Arduino) using the roboRIO's I2C port.",
"tags": [
"Hardware",
"I2C"
],
"foldername": "I2CCommunication",
"gradlebase": "cpp",
"hasunittests": true
},
{
"name": "Intermediate Vision",
"description": "Acquire images from an attached USB camera and add some annotation to the image (as you might do for showing operators the result of some image recognition) and send it to the dashboard for display.",
"tags": [
"Vision"
],
"foldername": "IntermediateVision",
"gradlebase": "cpp"
},
{
"name": "Motor Control",
"description": "Control a single motor with a joystick, displaying the movement of the motor using an encoder.",
"tags": [
"Basic Robot",
"Encoder",
"SmartDashboard",
"Joystick"
],
"foldername": "MotorControl",
"gradlebase": "cpp"
},
{
"name": "Select Command Example",
"description": "Use SelectCommand to select an autonomous routine.",
"tags": [
"Commandv2"
],
"foldername": "SelectCommand",
"gradlebase": "cpp",
"commandversion": 2
},
{
"name": "Simple Vision",
"description": "Use the CameraServer class to stream from a USB Webcam without processing the images.",
"tags": [
"Vision"
],
"foldername": "QuickVision",
"gradlebase": "cpp"
},
{
"name": "Solenoids",
"description": "Control a single and double solenoid from joystick buttons.",
"tags": [
"Hardware",
"Joystick",
"Pneumatics"
],
"foldername": "Solenoid",
"gradlebase": "cpp"
}
]