Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-01 02:41:48 +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

5
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java
View File

@@ -9,7 +9,6 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Ultrasonic;
import edu.wpi.first.wpilibj.event.BooleanEvent;
import edu.wpi.first.wpilibj.event.EventLoop;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
@@ -17,7 +16,6 @@ import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
public class Robot extends TimedRobot {
public static final int SHOT_VELOCITY = 200; // rpm
public static final int TOLERANCE = 8; // rpm
public static final int KICKER_THRESHOLD = 15; // mm
private final PWMSparkMax m_shooter = new PWMSparkMax(0);
private final Encoder m_shooterEncoder = new Encoder(0, 1);
@@ -25,7 +23,6 @@ public class Robot extends TimedRobot {
private final SimpleMotorFeedforward m_ff = new SimpleMotorFeedforward(0.1, 0.065);
private final PWMSparkMax m_kicker = new PWMSparkMax(1);
private final Ultrasonic m_kickerSensor = new Ultrasonic(2, 3);
private final PWMSparkMax m_intake = new PWMSparkMax(2);
@@ -37,7 +34,7 @@ public class Robot extends TimedRobot {
m_controller.setTolerance(TOLERANCE);
BooleanEvent isBallAtKicker =
new BooleanEvent(m_loop, () -> m_kickerSensor.getRangeMM() < KICKER_THRESHOLD);
new BooleanEvent(m_loop, () -> false); // m_kickerSensor.getRangeMM() < KICKER_THRESHOLD);
BooleanEvent intakeButton = new BooleanEvent(m_loop, () -> m_joystick.getRawButton(2));
// if the thumb button is held

27
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
View File

@@ -101,33 +101,6 @@
"mainclass": "Main",
"commandversion": 2
},
{
"name": "Ultrasonic",
"description": "View values from a ping-response ultrasonic sensor.",
"tags": [
"Hardware",
"Ultrasonic",
"SmartDashboard"
],
"foldername": "ultrasonic",
"gradlebase": "java",
"mainclass": "Main",
"commandversion": 2
},
{
"name": "Ultrasonic PID",
"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": "java",
"mainclass": "Main",
"commandversion": 2
},
{
"name": "Potentiometer PID",
"description": "Maintain elevator position setpoints with a potentiometer and PID control.",

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Main.java
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.
package edu.wpi.first.wpilibj.examples.ultrasonic;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

63
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java
View File

@@ -1,63 +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.
package edu.wpi.first.wpilibj.examples.ultrasonic;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Ultrasonic;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
* This is a sample program demonstrating how to read from a ping-response ultrasonic sensor with
* the {@link Ultrasonic class}.
*/
public class Robot extends TimedRobot {
// Creates a ping-response Ultrasonic object on DIO 1 and 2.
Ultrasonic m_rangeFinder = new Ultrasonic(1, 2);
/** Called once at the beginning of the robot program. */
public Robot() {
// Add the ultrasonic on the "Sensors" tab of the dashboard
// Data will update automatically
SmartDashboard.putData("Sensors", m_rangeFinder);
}
@Override
public void teleopPeriodic() {
// We can read the distance in millimeters
double distanceMillimeters = m_rangeFinder.getRangeMM();
// ... or in inches
double distanceInches = m_rangeFinder.getRangeInches();
// We can also publish the data itself periodically
SmartDashboard.putNumber("Distance[mm]", distanceMillimeters);
SmartDashboard.putNumber("Distance[inch]", distanceInches);
}
@Override
public void 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();
}
@Override
public void testPeriodic() {
if (m_rangeFinder.isRangeValid()) {
// Data is valid, publish it
SmartDashboard.putNumber("Distance[mm]", m_rangeFinder.getRangeMM());
SmartDashboard.putNumber("Distance[inch]", m_rangeFinder.getRangeInches());
// Ping for next measurement
m_rangeFinder.ping();
}
}
@Override
public void testExit() {
// Enable automatic mode
Ultrasonic.setAutomaticMode(true);
}
}

25
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Main.java
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.
package edu.wpi.first.wpilibj.examples.ultrasonicpid;
import edu.wpi.first.wpilibj.RobotBase;
/**
* Do NOT add any static variables to this class, or any initialization at all. Unless you know what
* you are doing, do not modify this file except to change the parameter class to the startRobot
* call.
*/
public final class Main {
private Main() {}
/**
* Main initialization function. Do not perform any initialization here.
*
* <p>If you change your main robot class, change the parameter type.
*/
public static void main(String... args) {
RobotBase.startRobot(Robot::new);
}
}

77
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
View File

@@ -1,77 +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.
package edu.wpi.first.wpilibj.examples.ultrasonicpid;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.filter.MedianFilter;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Ultrasonic;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
/**
* 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.
*/
public class Robot extends TimedRobot {
// distance the robot wants to stay from an object
// (one meter)
static final double kHoldDistanceMillimeters = 1.0e3;
// proportional speed constant
private static final double kP = 0.001;
// integral speed constant
private static final double kI = 0.0;
// derivative speed constant
private static final double kD = 0.0;
static final int kLeftMotorPort = 0;
static final int kRightMotorPort = 1;
static final int kUltrasonicPingPort = 0;
static final int kUltrasonicEchoPort = 1;
// Ultrasonic sensors tend to be quite noisy and susceptible to sudden outliers,
// so measurements are filtered with a 5-sample median filter
private final MedianFilter m_filter = new MedianFilter(5);
private final Ultrasonic m_ultrasonic = new Ultrasonic(kUltrasonicPingPort, kUltrasonicEchoPort);
private final PWMSparkMax m_leftMotor = new PWMSparkMax(kLeftMotorPort);
private final PWMSparkMax m_rightMotor = new PWMSparkMax(kRightMotorPort);
private final DifferentialDrive m_robotDrive =
new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);
private final PIDController m_pidController = new PIDController(kP, kI, kD);
public Robot() {
SendableRegistry.addChild(m_robotDrive, m_leftMotor);
SendableRegistry.addChild(m_robotDrive, m_rightMotor);
}
@Override
public void autonomousInit() {
// Set setpoint of the pid controller
m_pidController.setSetpoint(kHoldDistanceMillimeters);
}
@Override
public void autonomousPeriodic() {
double measurement = m_ultrasonic.getRangeMM();
double filteredMeasurement = m_filter.calculate(measurement);
double pidOutput = m_pidController.calculate(filteredMeasurement);
// disable input squaring -- PID output is linear
m_robotDrive.arcadeDrive(pidOutput, 0, false);
}
@Override
public void close() {
m_leftMotor.close();
m_rightMotor.close();
m_ultrasonic.close();
m_robotDrive.close();
super.close();
}
}

134
wpilibjExamples/src/test/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/UltrasonicPIDTest.java
View File

@@ -1,134 +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.
package edu.wpi.first.wpilibj.examples.ultrasonicpid;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.HAL.SimPeriodicBeforeCallback;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim.KitbotGearing;
import edu.wpi.first.wpilibj.simulation.DriverStationSim;
import edu.wpi.first.wpilibj.simulation.PWMSim;
import edu.wpi.first.wpilibj.simulation.SimHooks;
import edu.wpi.first.wpilibj.simulation.UltrasonicSim;
import org.junit.jupiter.api.AfterEach;
import org.junit.jupiter.api.parallel.ResourceLock;
import org.junit.jupiter.params.ParameterizedTest;
import org.junit.jupiter.params.provider.ValueSource;
@ResourceLock("timing")
class UltrasonicPIDTest {
private final DCMotor m_gearbox = DCMotor.getFalcon500(2);
private static final double kGearing = KitbotGearing.k10p71.value;
public static final double kvVoltSecondsPerMeter = 1.98;
public static final double kaVoltSecondsSquaredPerMeter = 0.2;
private static final double kvVoltSecondsPerRadian = 1.5;
private static final double kaVoltSecondsSquaredPerRadian = 0.3;
private static final double kWheelDiameterMeters = 0.15;
private static final double kTrackwidthMeters = 0.7;
private Robot m_robot;
private Thread m_thread;
private DifferentialDrivetrainSim m_driveSim;
private PWMSim m_leftMotorSim;
private PWMSim m_rightMotorSim;
private UltrasonicSim m_ultrasonicSim;
private SimPeriodicBeforeCallback m_callback;
// distance between the robot's starting position and the object
// we will update this in a moment
private double m_startToObject = Double.POSITIVE_INFINITY;
private double m_distanceMM;
// We're not using @BeforeEach so m_startToObject gets initialized properly
private void startThread() {
HAL.initialize(500, 0);
SimHooks.pauseTiming();
DriverStationSim.resetData();
m_robot = new Robot();
m_thread = new Thread(m_robot::startCompetition);
m_driveSim =
new DifferentialDrivetrainSim(
LinearSystemId.identifyDrivetrainSystem(
kvVoltSecondsPerMeter,
kaVoltSecondsSquaredPerMeter,
kvVoltSecondsPerRadian,
kaVoltSecondsSquaredPerRadian),
m_gearbox,
kGearing,
kTrackwidthMeters,
kWheelDiameterMeters / 2.0,
null);
m_ultrasonicSim = new UltrasonicSim(Robot.kUltrasonicPingPort, Robot.kUltrasonicEchoPort);
m_leftMotorSim = new PWMSim(Robot.kLeftMotorPort);
m_rightMotorSim = new PWMSim(Robot.kRightMotorPort);
m_callback =
HAL.registerSimPeriodicBeforeCallback(
() -> {
m_driveSim.setInputs(
m_leftMotorSim.getSpeed() * RobotController.getBatteryVoltage(),
m_rightMotorSim.getSpeed() * RobotController.getBatteryVoltage());
m_driveSim.update(0.02);
double startingDistance = m_startToObject;
double range = m_driveSim.getLeftPositionMeters() - startingDistance;
m_ultrasonicSim.setRangeMeters(range);
m_distanceMM = range * 1.0e3;
});
m_thread.start();
SimHooks.stepTiming(0.0); // Wait for Notifiers
SimHooks.stepTiming(0.02); // Have once iteration on disabled
}
@AfterEach
void stopThread() {
m_robot.endCompetition();
try {
m_thread.interrupt();
m_thread.join();
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();
}
m_robot.close();
m_callback.close();
m_leftMotorSim.resetData();
m_rightMotorSim.resetData();
}
@ValueSource(doubles = {1.3, 0.5, 5.0})
@ParameterizedTest
void autoTest(double distance) {
// set up distance
{
m_startToObject = distance;
}
startThread();
// auto init
{
DriverStationSim.setAutonomous(true);
DriverStationSim.setEnabled(true);
DriverStationSim.notifyNewData();
assertTrue(m_leftMotorSim.getInitialized());
assertTrue(m_rightMotorSim.getInitialized());
}
{
SimHooks.stepTiming(5.0);
assertEquals(Robot.kHoldDistanceMillimeters, m_distanceMM, 10.0);
}
}
}