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

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

@@ -115,11 +115,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": [
"Sensors",
"Robot and Motor",
"Analog"
"Hardware",
"Ultrasonic",
"SmartDashboard",
"Shuffleboard"
],
"foldername": "ultrasonic",
"gradlebase": "java",
@@ -131,8 +133,9 @@
"description": "Demonstrate maintaining a set distance using an ultrasonic sensor and PID Control.",
"tags": [
"Sensors",
"Robot and Motor",
"Analog"
"Ultrasonic",
"PID",
"Differential Drive"
],
"foldername": "ultrasonicpid",
"gradlebase": "java",
@@ -141,11 +144,13 @@
},
{
"name": "Potentiometer PID",
"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": [
"Sensors",
"Actuators",
"Analog",
"Elevator",
"PID",
"Joystick"
],
"foldername": "potentiometerpid",

64
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java
View File

@@ -5,7 +5,7 @@
package edu.wpi.first.wpilibj.examples.potentiometerpid;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.AnalogPotentiometer;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.motorcontrol.MotorController;
@@ -16,53 +16,55 @@ import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
* reach and maintain position setpoints on an elevator mechanism.
*/
public class Robot extends TimedRobot {
private static final int kPotChannel = 1;
private static final int kMotorChannel = 7;
private static final int kJoystickChannel = 0;
static final int kPotChannel = 1;
static final int kMotorChannel = 7;
static final int kJoystickChannel = 0;
// bottom, middle, and top elevator setpoints
private static final double[] kSetPoints = {1.0, 2.6, 4.3};
// The elevator can move 1.5 meters from top to bottom
static final double kFullHeightMeters = 1.5;
// proportional, integral, and derivative speed constants; motor inverted
// Bottom, middle, and top elevator setpoints
static final double[] kSetpointsMeters = {0.2, 0.8, 1.4};
// proportional, integral, and derivative speed constants
// DANGER: when tuning PID constants, high/inappropriate values for kP, kI,
// and kD may cause dangerous, uncontrollable, or undesired behavior!
// these may need to be positive for a non-inverted motor
private static final double kP = -5.0;
private static final double kI = -0.02;
private static final double kD = -2.0;
private static final double kP = 0.7;
private static final double kI = 0.35;
private static final double kD = 0.25;
private PIDController m_pidController;
private AnalogInput m_potentiometer;
private MotorController m_elevatorMotor;
private Joystick m_joystick;
private final PIDController m_pidController = new PIDController(kP, kI, kD);
// Scaling is handled internally
private final AnalogPotentiometer m_potentiometer =
new AnalogPotentiometer(kPotChannel, kFullHeightMeters);
private final MotorController m_elevatorMotor = new PWMSparkMax(kMotorChannel);
private final Joystick m_joystick = new Joystick(kJoystickChannel);
private int m_index;
private boolean m_previousButtonValue;
@Override
public void robotInit() {
m_potentiometer = new AnalogInput(kPotChannel);
m_elevatorMotor = new PWMSparkMax(kMotorChannel);
m_joystick = new Joystick(kJoystickChannel);
m_pidController = new PIDController(kP, kI, kD);
m_pidController.setSetpoint(kSetPoints[m_index]);
public void teleopInit() {
// Move to the bottom setpoint when teleop starts
m_index = 0;
m_pidController.setSetpoint(kSetpointsMeters[m_index]);
}
@Override
public void teleopPeriodic() {
// Read from the sensor
double position = m_potentiometer.get();
// Run the PID Controller
double pidOut = m_pidController.calculate(m_potentiometer.getAverageVoltage());
double pidOut = m_pidController.calculate(position);
// Apply PID output
m_elevatorMotor.set(pidOut);
// when the button is pressed once, the selected elevator setpoint
// is incremented
boolean currentButtonValue = m_joystick.getTrigger();
if (currentButtonValue && !m_previousButtonValue) {
// 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.length;
m_pidController.setSetpoint(kSetPoints[m_index]);
m_index = (m_index + 1) % kSetpointsMeters.length;
m_pidController.setSetpoint(kSetpointsMeters[m_index]);
}
m_previousButtonValue = currentButtonValue;
}
}

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

@@ -4,52 +4,61 @@
package edu.wpi.first.wpilibj.examples.ultrasonic;
import edu.wpi.first.math.filter.MedianFilter;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
import edu.wpi.first.wpilibj.Ultrasonic;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
* This is a sample program demonstrating how to use an ultrasonic sensor and proportional control
* to maintain a set distance from an object.
* 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 {
// distance in inches the robot wants to stay from an object
private static final double kHoldDistance = 12.0;
// Creates a ping-response Ultrasonic object on DIO 1 and 2.
Ultrasonic m_rangeFinder = new Ultrasonic(1, 2);
// factor to convert sensor values to a distance in inches
private static final double kValueToInches = 0.125;
@Override
public void robotInit() {
// Add the ultrasonic on the "Sensors" tab of the dashboard
// Data will update automatically
Shuffleboard.getTab("Sensors").add(m_rangeFinder);
}
// proportional speed constant
private static final double kP = 0.05;
private static final int kLeftMotorPort = 0;
private static final int kRightMotorPort = 1;
private static final int kUltrasonicPort = 0;
// median filter to discard outliers; filters over 10 samples
private final MedianFilter m_filter = new MedianFilter(10);
private final AnalogInput m_ultrasonic = new AnalogInput(kUltrasonicPort);
private final DifferentialDrive m_robotDrive =
new DifferentialDrive(new PWMSparkMax(kLeftMotorPort), new PWMSparkMax(kRightMotorPort));
/**
* Tells the robot to drive to a set distance (in inches) from an object using proportional
* control.
*/
@Override
public void teleopPeriodic() {
// 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.getValue()) * kValueToInches;
// We can read the distance in millimeters
double distanceMillimeters = m_rangeFinder.getRangeMM();
// ... or in inches
double distanceInches = m_rangeFinder.getRangeInches();
// convert distance error to a motor speed
double currentSpeed = (kHoldDistance - currentDistance) * kP;
// We can also publish the data itself periodically
SmartDashboard.putNumber("Distance[mm]", distanceMillimeters);
SmartDashboard.putNumber("Distance[inch]", distanceInches);
}
// drive robot
m_robotDrive.arcadeDrive(currentSpeed, 0);
@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);
}
}

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

@@ -6,8 +6,8 @@ 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.wpilibj.AnalogInput;
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;
@@ -16,45 +16,56 @@ import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
* reach and maintain a set distance from an object.
*/
public class Robot extends TimedRobot {
// distance in inches the robot wants to stay from an object
private static final double kHoldDistance = 12.0;
// factor to convert sensor values to a distance in inches
private static final double kValueToInches = 0.125;
// 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 = 7.0;
// negative because applying positive voltage will bring us closer to the target
private static final double kP = -0.001;
// integral speed constant
private static final double kI = 0.018;
private static final double kI = 0.0;
// derivative speed constant
private static final double kD = 1.5;
private static final double kD = 0.0;
private static final int kLeftMotorPort = 0;
private static final int kRightMotorPort = 1;
private static final int kUltrasonicPort = 0;
static final int kLeftMotorPort = 0;
static final int kRightMotorPort = 1;
// median filter to discard outliers; filters over 5 samples
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 AnalogInput m_ultrasonic = new AnalogInput(kUltrasonicPort);
private final DifferentialDrive m_robotDrive =
new DifferentialDrive(new PWMSparkMax(kLeftMotorPort), new PWMSparkMax(kRightMotorPort));
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, m_rightMotor);
private final PIDController m_pidController = new PIDController(kP, kI, kD);
@Override
public void teleopInit() {
public void autonomousInit() {
// Set setpoint of the pid controller
m_pidController.setSetpoint(kHoldDistance * kValueToInches);
m_pidController.setSetpoint(kHoldDistanceMillimeters);
}
@Override
public void teleopPeriodic() {
// returned value is filtered with a rolling median filter, since ultrasonics
// tend to be quite noisy and susceptible to sudden outliers
double pidOutput = m_pidController.calculate(m_filter.calculate(m_ultrasonic.getVoltage()));
public void autonomousPeriodic() {
double measurement = m_ultrasonic.getRangeMM();
double filteredMeasurement = m_filter.calculate(measurement);
double pidOutput = m_pidController.calculate(filteredMeasurement);
m_robotDrive.arcadeDrive(pidOutput, 0);
// 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();
}
}