Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib, examples] Cleanup PotentiometerPID, Ultrasonic, UltrasonicPID examples (#4893)

Browse Source 
Fix C++ Ultrasonic to return correct units.
This commit is contained in:
Starlight220
2023-01-09 02:33:07 +02:00
committed by GitHub
parent babb0c1fcf
commit 2cd9be413f
24 changed files with 1052 additions and 301 deletions
Download Patch File Download Diff File Expand all files Collapse all files

86
wpilibcExamples/src/main/cpp/examples/PotentiometerPID/cpp/Robot.cpp
View File

@@ -2,72 +2,32 @@
// 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 <array>
#include "Robot.h"
#include <frc/AnalogInput.h>
#include <frc/Joystick.h>
#include <frc/TimedRobot.h>
#include <frc/controller/PIDController.h>
#include <frc/motorcontrol/PWMSparkMax.h>
void Robot::TeleopInit() {
// Move to the bottom setpoint when teleop starts
m_index = 0;
m_pidController.SetSetpoint(kSetpoints[m_index].value());
}
/**
* This is a sample program to demonstrate how to use a soft potentiometer and a
* PID Controller to reach and maintain position setpoints on an elevator
* mechanism.
*/
class Robot : public frc::TimedRobot {
public:
void RobotInit() override {
m_pidController.SetSetpoint(kSetPoints[m_index]);
void Robot::TeleopPeriodic() {
// Read from the sensor
units::meter_t position = units::meter_t{m_potentiometer.Get()};
// Run the PID Controller
double pidOut = m_pidController.Calculate(position.value());
// Apply PID output
m_elevatorMotor.Set(pidOut);
// when the button is pressed once, the selected elevator setpoint is
// incremented
if (m_joystick.GetTriggerPressed()) {
// index of the elevator setpoint wraps around.
m_index = (m_index + 1) % kSetpoints.size();
m_pidController.SetSetpoint(kSetpoints[m_index].value());
}
void TeleopPeriodic() override {
// When the button is pressed once, the selected elevator setpoint is
// incremented.
bool currentButtonValue = m_joystick.GetTrigger();
if (currentButtonValue && !m_previousButtonValue) {
// Index of the elevator setpoint wraps around
m_index = (m_index + 1) % (sizeof(kSetPoints) / 8);
m_pidController.SetSetpoint(kSetPoints[m_index]);
}
m_previousButtonValue = currentButtonValue;
double output =
m_pidController.Calculate(m_potentiometer.GetAverageVoltage());
m_elevatorMotor.Set(output);
}
private:
static constexpr int kPotChannel = 1;
static constexpr int kMotorChannel = 7;
static constexpr int kJoystickChannel = 0;
// Bottom, middle, and top elevator setpoints
static constexpr std::array<double, 3> kSetPoints = {{1.0, 2.6, 4.3}};
/* Proportional, integral, and derivative speed constants; motor inverted.
*
* DANGER: When tuning PID constants, high/inappropriate values for pGain,
* iGain, and dGain may cause dangerous, uncontrollable, or undesired
* behavior!
*
* These may need to be positive for a non-inverted motor.
*/
static constexpr double kP = -5.0;
static constexpr double kI = -0.02;
static constexpr double kD = -2.0;
int m_index = 0;
bool m_previousButtonValue = false;
frc::AnalogInput m_potentiometer{kPotChannel};
frc::Joystick m_joystick{kJoystickChannel};
frc::PWMSparkMax m_elevatorMotor{kMotorChannel};
frc2::PIDController m_pidController{kP, kI, kD};
};
constexpr std::array<double, 3> Robot::kSetPoints;
}
#ifndef RUNNING_FRC_TESTS
int main() {

55
wpilibcExamples/src/main/cpp/examples/PotentiometerPID/include/Robot.h Normal file
View File

@@ -0,0 +1,55 @@
// 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 <frc/AnalogPotentiometer.h>
#include <frc/Joystick.h>
#include <frc/TimedRobot.h>
#include <frc/controller/PIDController.h>
#include <frc/motorcontrol/PWMSparkMax.h>
#include <units/length.h>
/**
* This is a sample program to demonstrate how to use a soft potentiometer and a
* PID controller to reach and maintain position setpoints on an elevator
* mechanism.
*/
class Robot : public frc::TimedRobot {
public:
void TeleopInit() override;
void TeleopPeriodic() override;
static constexpr int kPotChannel = 1;
static constexpr int kMotorChannel = 7;
static constexpr int kJoystickChannel = 0;
// The elevator can move 1.5 meters from top to bottom
static constexpr units::meter_t kFullHeight = 1.5_m;
// Bottom, middle, and top elevator setpoints
static constexpr std::array<units::meter_t, 3> kSetpoints = {
{0.2_m, 0.8_m, 1.4_m}};
private:
// proportional speed constant
// negative because applying positive voltage will bring us closer to the
// target
static constexpr double kP = 0.7;
// integral speed constant
static constexpr double kI = 0.35;
// derivative speed constant
static constexpr double kD = 0.25;
// Scaling is handled internally
frc::AnalogPotentiometer m_potentiometer{kPotChannel, kFullHeight.value()};
frc::PWMSparkMax m_elevatorMotor{kMotorChannel};
frc2::PIDController m_pidController{kP, kI, kD};
frc::Joystick m_joystick{kJoystickChannel};
size_t m_index;
};

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

@@ -2,57 +2,54 @@
// 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 <frc/AnalogInput.h>
#include <frc/TimedRobot.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/filter/MedianFilter.h>
#include <frc/motorcontrol/PWMSparkMax.h>
#include "Robot.h"
/**
* This is a sample program demonstrating how to use an ultrasonic sensor and
* proportional control to maintain a set distance from an object.
*/
class Robot : public frc::TimedRobot {
public:
/**
* Tells the robot to drive to a set distance (in inches) from an object using
* proportional control.
*/
void TeleopPeriodic() override {
// Sensor returns a value from 0-4095 that is scaled to inches
// returned value is filtered with a rolling median filter, since
// ultrasonics tend to be quite noisy and susceptible to sudden outliers
double currentDistance =
m_filter.Calculate(m_ultrasonic.GetVoltage()) * kValueToInches;
// Convert distance error to a motor speed
double currentSpeed = (kHoldDistance - currentDistance) * kP;
// Drive robot
m_robotDrive.ArcadeDrive(currentSpeed, 0);
#include <frc/shuffleboard/Shuffleboard.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include <units/length.h>
void Robot::RobotInit() {
// Add the ultrasonic on the "Sensors" tab of the dashboard
// Data will update automatically
frc::Shuffleboard::GetTab("Sensors").Add(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();
}
}
private:
// Distance in inches the robot wants to stay from an object
static constexpr int kHoldDistance = 12;
// Factor to convert sensor values to a distance in inches
static constexpr double kValueToInches = 0.125;
// Proportional speed constant
static constexpr double kP = 0.05;
static constexpr int kLeftMotorPort = 0;
static constexpr int kRightMotorPort = 1;
static constexpr int kUltrasonicPort = 0;
// median filter to discard outliers; filters over 10 samples
frc::MedianFilter<double> m_filter{10};
frc::AnalogInput m_ultrasonic{kUltrasonicPort};
frc::PWMSparkMax m_left{kLeftMotorPort};
frc::PWMSparkMax m_right{kRightMotorPort};
frc::DifferentialDrive m_robotDrive{m_left, m_right};
};
void Robot::TestExit() {
// Enable automatic mode
frc::Ultrasonic::SetAutomaticMode(true);
}
#ifndef RUNNING_FRC_TESTS
int main() {

25
wpilibcExamples/src/main/cpp/examples/Ultrasonic/include/Robot.h Normal file
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 <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:
void RobotInit() override;
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};
};

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

@@ -2,65 +2,21 @@
// 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 <frc/AnalogInput.h>
#include <frc/TimedRobot.h>
#include <frc/controller/PIDController.h>
#include <frc/drive/DifferentialDrive.h>
#include <frc/filter/MedianFilter.h>
#include <frc/motorcontrol/PWMSparkMax.h>
#include "Robot.h"
/**
* This is a sample program demonstrating how to use an ultrasonic sensor and
* proportional control to maintain a set distance from an object.
*/
class Robot : public frc::TimedRobot {
public:
/**
* Drives the robot a set distance from an object using PID control and the
* ultrasonic sensor.
*/
void TeleopInit() override {
// Set setpoint of the PID Controller
m_pidController.SetSetpoint(kHoldDistance * kValueToInches);
}
void Robot::AutonomousInit() {
// Set setpoint of the pid controller
m_pidController.SetSetpoint(kHoldDistance.value());
}
void TeleopPeriodic() override {
double output =
m_pidController.Calculate(m_filter.Calculate(m_ultrasonic.GetValue()));
m_robotDrive.ArcadeDrive(output, 0);
}
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());
private:
// Distance in inches the robot wants to stay from an object
static constexpr int kHoldDistance = 12;
// Factor to convert sensor values to a distance in inches
static constexpr double kValueToInches = 0.125;
// proportional speed constant
static constexpr double kP = 7.0;
// integral speed constant
static constexpr double kI = 0.018;
// derivative speed constant
static constexpr double kD = 1.5;
static constexpr int kLeftMotorPort = 0;
static constexpr int kRightMotorPort = 1;
static constexpr int kUltrasonicPort = 0;
// median filter to discard outliers; filters over 5 samples
frc::MedianFilter<double> m_filter{5};
frc::AnalogInput m_ultrasonic{kUltrasonicPort};
frc::PWMSparkMax m_left{kLeftMotorPort};
frc::PWMSparkMax m_right{kRightMotorPort};
frc::DifferentialDrive m_robotDrive{m_left, m_right};
frc2::PIDController m_pidController{kP, kI, kD};
};
// disable input squaring -- PID output is linear
m_robotDrive.ArcadeDrive(pidOutput, 0, false);
}
#ifndef RUNNING_FRC_TESTS
int main() {

51
wpilibcExamples/src/main/cpp/examples/UltrasonicPID/include/Robot.h Normal file
View File

@@ -0,0 +1,51 @@
// 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:
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
// negative because applying positive voltage will bring us closer to the
// target
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{m_left, m_right};
frc2::PIDController m_pidController{kP, kI, kD};
};

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

@@ -122,12 +122,13 @@
},
{
"name": "Ultrasonic",
"description": "Demonstrate maintaining a set distance using an ultrasonic sensor.",
"description": "Demonstrate using the Ultrasonic class with a ping-response ultrasonic sensor.",
"tags": [
"Robot and Motor",
"Complete List",
"Sensors",
"Analog"
"Hardware",
"Ultrasonic",
"SmartDashboard",
"Shuffleboard"
],
"foldername": "Ultrasonic",
"gradlebase": "cpp",
@@ -135,12 +136,12 @@
},
{
"name": "UltrasonicPID",
"description": "Demonstrate maintaining a set distance using an ultrasonic sensor and PID control.",
"description": "Demonstrate maintaining a set distance using an ultrasonic sensor and PID Control.",
"tags": [
"Robot and Motor",
"Complete List",
"Sensors",
"Analog"
"Ultrasonic",
"PID",
"Differential Drive"
],
"foldername": "UltrasonicPID",
"gradlebase": "cpp",
@@ -186,11 +187,12 @@
},
{
"name": "PotentiometerPID",
"description": "An example to demonstrate the use of a potentiometer and PID control to reach elevator position setpoints.",
"description": "An example to demonstrate the use of a potentiometer and PID control to maintain elevator position setpoints.",
"tags": [
"Joystick",
"Actuators",
"Complete List",
"PID",
"Elevator",
"Sensors",
"Analog"
],