Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[hal, wpilib] Add initial systemcore counter implementation (#7723)

Browse Source
This commit is contained in:
Thad House
2025-01-28 08:58:34 -08:00
committed by GitHub
parent b799b285b3
commit 48ce2dcc8d
47 changed files with 201 additions and 4357 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
wpilibcExamples/src/main/cpp/examples/EventLoop/cpp/Robot.cpp
View File

@@ -5,7 +5,6 @@
#include <frc/Encoder.h>
#include <frc/Joystick.h>
#include <frc/TimedRobot.h>
#include <frc/Ultrasonic.h>
#include <frc/controller/PIDController.h>
#include <frc/controller/SimpleMotorFeedforward.h>
#include <frc/event/BooleanEvent.h>
@@ -18,16 +17,16 @@
static const auto SHOT_VELOCITY = 200_rpm;
static const auto TOLERANCE = 8_rpm;
static const auto KICKER_THRESHOLD = 15_mm;
class Robot : public frc::TimedRobot {
public:
Robot() {
m_controller.SetTolerance(TOLERANCE.value());
frc::BooleanEvent isBallAtKicker{&m_loop, [&kickerSensor = m_kickerSensor] {
return kickerSensor.GetRange() <
KICKER_THRESHOLD;
frc::BooleanEvent isBallAtKicker{&m_loop, [] {
return false;
// return kickerSensor.GetRange() <
// KICKER_THRESHOLD;
}};
frc::BooleanEvent intakeButton{
&m_loop, [&joystick = m_joystick] { return joystick.GetRawButton(2); }};
@@ -88,7 +87,6 @@ class Robot : public frc::TimedRobot {
frc::SimpleMotorFeedforward<units::radians> m_ff{0.1_V, 0.065_V / 1_rpm};
frc::PWMSparkMax m_kicker{1};
frc::Ultrasonic m_kickerSensor{2, 3};
frc::PWMSparkMax m_intake{3};

57
wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp
View File

@@ -1,57 +0,0 @@
// 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.h"
#include <frc/smartdashboard/SmartDashboard.h>
#include <units/length.h>
Robot::Robot() {
// Add the ultrasonic on the "Sensors" tab of the dashboard
// Data will update automatically
frc::SmartDashboard::PutData("Sensors", &m_rangeFinder);
}
void Robot::TeleopPeriodic() {
// We can read the distance
units::meter_t distance = m_rangeFinder.GetRange();
// units auto-convert
units::millimeter_t distanceMillimeters = distance;
units::inch_t distanceInches = distance;
// We can also publish the data itself periodically
frc::SmartDashboard::PutNumber("Distance[mm]", distanceMillimeters.value());
frc::SmartDashboard::PutNumber("Distance[inch]", distanceInches.value());
}
void Robot::TestInit() {
// By default, the Ultrasonic class polls all ultrasonic sensors in a
// round-robin to prevent them from interfering from one another. However,
// manual polling is also possible -- note that this disables automatic mode!
m_rangeFinder.Ping();
}
void Robot::TestPeriodic() {
if (m_rangeFinder.IsRangeValid()) {
// Data is valid, publish it
units::millimeter_t distanceMillimeters = m_rangeFinder.GetRange();
units::inch_t distanceInches = m_rangeFinder.GetRange();
frc::SmartDashboard::PutNumber("Distance[mm]", distanceMillimeters.value());
frc::SmartDashboard::PutNumber("Distance[inch]", distanceInches.value());
// Ping for next measurement
m_rangeFinder.Ping();
}
}
void Robot::TestExit() {
// Enable automatic mode
frc::Ultrasonic::SetAutomaticMode(true);
}
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
}
#endif

25
wpilibcExamples/src/main/cpp/examples/Ultrasonic/include/Robot.h
View File

@@ -1,25 +0,0 @@
// 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 <frc/TimedRobot.h>
#include <frc/Ultrasonic.h>
/**
* This is a sample program demonstrating how to read from a ping-response
* ultrasonic sensor with the {@link Ultrasonic class}.
*/
class Robot : public frc::TimedRobot {
public:
Robot();
void TeleopPeriodic() override;
void TestInit() override;
void TestPeriodic() override;
void TestExit() override;
private:
// Creates a ping-response Ultrasonic object on DIO 1 and 2.
frc::Ultrasonic m_rangeFinder{1, 2};
};

30
wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
View File

@@ -1,30 +0,0 @@
// 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.h"
Robot::Robot() {
wpi::SendableRegistry::AddChild(&m_robotDrive, &m_left);
wpi::SendableRegistry::AddChild(&m_robotDrive, &m_right);
}
void Robot::AutonomousInit() {
// Set setpoint of the pid controller
m_pidController.SetSetpoint(kHoldDistance.value());
}
void Robot::AutonomousPeriodic() {
units::millimeter_t measurement = m_ultrasonic.GetRange();
units::millimeter_t filteredMeasurement = m_filter.Calculate(measurement);
double pidOutput = m_pidController.Calculate(filteredMeasurement.value());
// disable input squaring -- PID output is linear
m_robotDrive.ArcadeDrive(pidOutput, 0, false);
}
#ifndef RUNNING_FRC_TESTS
int main() {
return frc::StartRobot<Robot>();
}
#endif

52
wpilibcExamples/src/main/cpp/examples/UltrasonicPID/include/Robot.h
View File

@@ -1,52 +0,0 @@
// 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 <frc/TimedRobot.h>
#include <frc/Ultrasonic.h>
#include <frc/controller/PIDController.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/filter/MedianFilter.h>
#include <frc/motorcontrol/PWMSparkMax.h>
#include <units/length.h>
/**
* This is a sample program to demonstrate the use of a PIDController with an
* ultrasonic sensor to reach and maintain a set distance from an object.
*/
class Robot : public frc::TimedRobot {
public:
Robot();
void AutonomousInit() override;
void AutonomousPeriodic() override;
// distance the robot wants to stay from an object
static constexpr units::millimeter_t kHoldDistance = 1_m;
static constexpr int kLeftMotorPort = 0;
static constexpr int kRightMotorPort = 1;
static constexpr int kUltrasonicPingPort = 0;
static constexpr int kUltrasonicEchoPort = 1;
private:
// proportional speed constant
static constexpr double kP = 0.001;
// integral speed constant
static constexpr double kI = 0.0;
// derivative speed constant
static constexpr double kD = 0.0;
// Ultrasonic sensors tend to be quite noisy and susceptible to sudden
// outliers, so measurements are filtered with a 5-sample median filter
frc::MedianFilter<units::millimeter_t> m_filter{5};
frc::Ultrasonic m_ultrasonic{kUltrasonicPingPort, kUltrasonicEchoPort};
frc::PWMSparkMax m_left{kLeftMotorPort};
frc::PWMSparkMax m_right{kRightMotorPort};
frc::DifferentialDrive m_robotDrive{
[&](double output) { m_left.Set(output); },
[&](double output) { m_right.Set(output); }};
frc::PIDController m_pidController{kP, kI, kD};
};

25
wpilibcExamples/src/main/cpp/examples/examples.json
View File

@@ -112,31 +112,6 @@
"gradlebase": "cpp",
"commandversion": 2
},
{
"name": "Ultrasonic",
"description": "View values from a ping-response ultrasonic sensor.",
"tags": [
"Hardware",
"Ultrasonic",
"SmartDashboard"
],
"foldername": "Ultrasonic",
"gradlebase": "cpp",
"commandversion": 2
},
{
"name": "UltrasonicPID",
"description": "Maintain a set distance from an obstacle with an ultrasonic sensor and PID control.",
"tags": [
"Basic Robot",
"Ultrasonic",
"PID",
"Differential Drive"
],
"foldername": "UltrasonicPID",
"gradlebase": "cpp",
"commandversion": 2
},
{
"name": "Gyro",
"description": "Drive a differential drive straight with a gyro sensor.",

109
wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/UltrasonicPIDTest.cpp
View File

@@ -1,109 +0,0 @@
// 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 <string>
#include <thread>
#include <frc/RobotController.h>
#include <frc/simulation/DifferentialDrivetrainSim.h>
#include <frc/simulation/DriverStationSim.h>
#include <frc/simulation/PWMSim.h>
#include <frc/simulation/SimHooks.h>
#include <frc/simulation/UltrasonicSim.h>
#include <frc/system/plant/DCMotor.h>
#include <frc/system/plant/LinearSystemId.h>
#include <gtest/gtest.h>
#include <hal/simulation/MockHooks.h>
#include <units/angle.h>
#include <units/length.h>
#include <units/mass.h>
#include <units/time.h>
#include "Robot.h"
class UltrasonicPIDTest : public testing::TestWithParam<double> {
frc::DCMotor m_gearbox = frc::DCMotor::Falcon500(2);
static constexpr auto kGearing =
frc::sim::DifferentialDrivetrainSim::KitbotGearing::k10p71;
static constexpr auto kvLinear = 1.98 * 1_V / 1_mps;
static constexpr auto kaLinear = 0.2 * 1_V / 1_mps_sq;
static constexpr auto kvVoltAngular = 1.5 * 1_V / 1_rad_per_s;
static constexpr auto kaAngular = 0.3 * 1_V / 1_rad_per_s_sq;
static constexpr auto kWheelDiameter = 0.15_m;
static constexpr auto kTrackwidth = 0.7_m;
Robot m_robot;
std::optional<std::thread> m_thread;
protected:
frc::sim::DifferentialDrivetrainSim m_driveSim{
frc::LinearSystemId::IdentifyDrivetrainSystem(
kvLinear, kaLinear, kvVoltAngular, kaAngular, kTrackwidth),
kTrackwidth, m_gearbox, kGearing, kWheelDiameter / 2.0};
frc::sim::PWMSim m_leftMotorSim{Robot::kLeftMotorPort};
frc::sim::PWMSim m_rightMotorSim{Robot::kRightMotorPort};
frc::sim::UltrasonicSim m_ultrasonicSim{Robot::kUltrasonicPingPort,
Robot::kUltrasonicEchoPort};
int32_t m_callback;
units::millimeter_t m_distance;
public:
void SimPeriodicBefore() {
m_driveSim.SetInputs(
m_leftMotorSim.GetSpeed() * frc::RobotController::GetBatteryVoltage(),
m_rightMotorSim.GetSpeed() * frc::RobotController::GetBatteryVoltage());
m_driveSim.Update(20_ms);
auto startingDistance = units::meter_t{GetParam()};
m_distance = m_driveSim.GetLeftPosition() - startingDistance;
m_ultrasonicSim.SetRange(m_distance);
}
static void CallSimPeriodicBefore(void* param) {
static_cast<UltrasonicPIDTest*>(param)->SimPeriodicBefore();
}
void SetUp() override {
frc::sim::PauseTiming();
frc::sim::DriverStationSim::ResetData();
m_callback =
HALSIM_RegisterSimPeriodicBeforeCallback(CallSimPeriodicBefore, this);
m_thread = std::thread([&] { m_robot.StartCompetition(); });
frc::sim::StepTiming(0.0_ms); // Wait for Notifiers
}
void TearDown() override {
m_robot.EndCompetition();
m_thread->join();
HALSIM_CancelSimPeriodicBeforeCallback(m_callback);
m_leftMotorSim.ResetData();
m_rightMotorSim.ResetData();
}
};
TEST_P(UltrasonicPIDTest, Auto) {
// auto init
{
frc::sim::DriverStationSim::SetAutonomous(true);
frc::sim::DriverStationSim::SetEnabled(true);
frc::sim::DriverStationSim::NotifyNewData();
EXPECT_TRUE(m_leftMotorSim.GetInitialized());
EXPECT_TRUE(m_rightMotorSim.GetInitialized());
}
{
frc::sim::StepTiming(5_s);
EXPECT_NEAR(Robot::kHoldDistance.value(), m_distance.value(), 10.0);
}
}
INSTANTIATE_TEST_SUITE_P(UltrasonicPIDTests, UltrasonicPIDTest,
testing::Values(1.3, 0.5, 5.0));

16
wpilibcExamples/src/test/cpp/examples/UltrasonicPID/cpp/main.cpp
View File

@@ -1,16 +0,0 @@
// 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 <gtest/gtest.h>
#include <hal/HALBase.h>
/**
* Runs all unit tests.
*/
int main(int argc, char** argv) {
HAL_Initialize(500, 0);
::testing::InitGoogleTest(&argc, argv);
int ret = RUN_ALL_TESTS();
return ret;
}