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

Add setVoltage method to SpeedController (#1997)

Browse Source 
Add a voltage-compensated setVoltage method to SpeedController, which is sorely needed to help teams use feedforward-based controls effectively. Also uses correct units on the cpp side.

Also update relevant examples.
This commit is contained in:
Oblarg
2019-11-01 12:32:40 -04:00
committed by Peter Johnson
parent f6e311ef86
commit 9ebd23d61e
12 changed files with 88 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
wpilibc/src/main/native/cpp/SpeedController.cpp Normal file
View File

@@ -0,0 +1,16 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "frc/SpeedController.h"
#include <frc/RobotController.h>
using namespace frc;
void SpeedController::SetVoltage(units::volt_t output) {
Set(output / units::volt_t(RobotController::GetInputVoltage()));
}

18
wpilibc/src/main/native/include/frc/SpeedController.h
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -7,6 +7,8 @@
#pragma once
#include <units/units.h>
#include "frc/PIDOutput.h"
namespace frc {
@@ -25,6 +27,20 @@ class SpeedController : public PIDOutput {
*/
virtual void Set(double speed) = 0;
/**
* Sets the voltage output of the SpeedController. Compensates for
* the current bus voltage to ensure that the desired voltage is output even
* if the battery voltage is below 12V - highly useful when the voltage
* outputs are "meaningful" (e.g. they come from a feedforward calculation).
*
* <p>NOTE: This function *must* be called regularly in order for voltage
* compensation to work properly - unlike the ordinary set function, it is not
* "set it and forget it."
*
* @param output The voltage to output.
*/
virtual void SetVoltage(units::volt_t output);
/**
* Common interface for getting the current set speed of a speed controller.
*

9
wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp
View File

@@ -18,13 +18,14 @@ ShooterSubsystem::ShooterSubsystem()
m_shooterMotor(kShooterMotorPort),
m_feederMotor(kFeederMotorPort),
m_shooterEncoder(kEncoderPorts[0], kEncoderPorts[1]) {
m_controller.SetTolerance(kShooterToleranceRPS);
m_controller.SetTolerance(kShooterToleranceRPS.to<double>());
m_shooterEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
}
void ShooterSubsystem::UseOutput(double output) {
// Use a feedforward of the form kS + kV * velocity
m_shooterMotor.Set(output + kSFractional + kVFractional * kShooterTargetRPS);
m_shooterMotor.SetVoltage(units::volt_t(output) + kS +
kV * kShooterTargetRPS);
}
void ShooterSubsystem::Disable() {
@@ -38,7 +39,9 @@ bool ShooterSubsystem::AtSetpoint() { return m_controller.AtSetpoint(); }
double ShooterSubsystem::GetMeasurement() { return m_shooterEncoder.GetRate(); }
double ShooterSubsystem::GetSetpoint() { return kShooterTargetRPS; }
double ShooterSubsystem::GetSetpoint() {
return kShooterTargetRPS.to<double>();
}
void ShooterSubsystem::RunFeeder() { m_feederMotor.Set(kFeederSpeed); }

14
wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
View File

@@ -47,9 +47,9 @@ constexpr double kEncoderDistancePerPulse =
constexpr int kShooterMotorPort = 4;
constexpr int kFeederMotorPort = 5;
constexpr double kShooterFreeRPS = 5300;
constexpr double kShooterTargetRPS = 4000;
constexpr double kShooterToleranceRPS = 50;
constexpr auto kShooterFreeRPS = 5300_tr / 1_s;
constexpr auto kShooterTargetRPS = 4000_tr / 1_s;
constexpr auto kShooterToleranceRPS = 50_tr / 1_s;
constexpr double kP = 1;
constexpr double kI = 0;
@@ -57,10 +57,10 @@ constexpr double kD = 0;
// On a real robot the feedforward constants should be empirically determined;
// these are reasonable guesses.
constexpr double kSFractional = .05;
constexpr double kVFractional =
// Should have value 1 at free speed...
1. / kShooterFreeRPS;
constexpr auto kS = .05_V;
constexpr auto kV =
// Should have value 12V at free speed...
12_V / kShooterFreeRPS;
constexpr double kFeederSpeed = .5;
} // namespace ShooterConstants

6
wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
View File

@@ -75,13 +75,11 @@ frc2::Command* RobotContainer::GetAutonomousCommand() {
},
frc2::PIDController(DriveConstants::kPDriveVel, 0, 0),
frc2::PIDController(DriveConstants::kPDriveVel, 0, 0),
[this](auto left, auto right) {
m_drive.TankDrive(left / 12_V, right / 12_V);
},
[this](auto left, auto right) { m_drive.TankDriveVolts(left, right); },
{&m_drive});
// no auto
return new frc2::SequentialCommandGroup(
std::move(ramseteCommand),
frc2::InstantCommand([this] { m_drive.TankDrive(0, 0); }, {}));
frc2::InstantCommand([this] { m_drive.TankDriveVolts(0_V, 0_V); }, {}));
}

7
wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
View File

@@ -7,8 +7,6 @@
#include "subsystems/DriveSubsystem.h"
#include <units/units.h>
#include <frc/geometry/Rotation2d.h>
#include <frc/kinematics/DifferentialDriveWheelSpeeds.h>
@@ -39,8 +37,9 @@ void DriveSubsystem::ArcadeDrive(double fwd, double rot) {
m_drive.ArcadeDrive(fwd, rot);
}
void DriveSubsystem::TankDrive(double left, double right) {
m_drive.TankDrive(left, right, false);
void DriveSubsystem::TankDriveVolts(units::volt_t left, units::volt_t right) {
m_leftMotors.SetVoltage(left);
m_rightMotors.SetVoltage(-right);
}
void DriveSubsystem::ResetEncoders() {

7
wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
View File

@@ -7,6 +7,8 @@
#pragma once
#include <units/units.h>
#include <frc/ADXRS450_Gyro.h>
#include <frc/Encoder.h>
#include <frc/PWMVictorSPX.h>
@@ -38,13 +40,12 @@ class DriveSubsystem : public frc2::SubsystemBase {
void ArcadeDrive(double fwd, double rot);
/**
* Drives the robot using tank controls. Does not square inputs to enable
* composition with external controllers.
* Controls each side of the drive directly with a voltage.
*
* @param left the commanded left output
* @param right the commanded right output
*/
void TankDrive(double left, double right);
void TankDriveVolts(units::volt_t left, units::volt_t right);
/**
* Resets the drive encoders to currently read a position of 0.

17
wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedController.java
View File

@@ -1,5 +1,5 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
@@ -18,6 +18,21 @@ public interface SpeedController extends PIDOutput {
*/
void set(double speed);
/**
* Sets the voltage output of the SpeedController. Compensates for the current bus
* voltage to ensure that the desired voltage is output even if the battery voltage is below
* 12V - highly useful when the voltage outputs are "meaningful" (e.g. they come from a
* feedforward calculation).
*
* <p>NOTE: This function *must* be called regularly in order for voltage compensation to work
* properly - unlike the ordinary set function, it is not "set it and forget it."
*
* @param outputVolts The voltage to output.
*/
default void setVoltage(double outputVolts) {
set(outputVolts / RobotController.getBatteryVoltage());
}
/**
* Common interface for getting the current set speed of a speed controller.
*

9
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java
View File

@@ -49,16 +49,17 @@ public final class Constants {
public static final double kShooterTargetRPS = 4000;
public static final double kShooterToleranceRPS = 50;
// These are not real PID gains, and will have to be tuned for your specific robot.
public static final double kP = 1;
public static final double kI = 0;
public static final double kD = 0;
// On a real robot the feedforward constants should be empirically determined; these are
// reasonable guesses.
public static final double kSFractional = .05;
public static final double kVFractional =
// Should have value 1 at free speed...
1. / kShooterFreeRPS;
public static final double kSVolts = .05;
public static final double kVVoltSecondsPerRotation =
// Should have value 12V at free speed...
12. / kShooterFreeRPS;
public static final double kFeederSpeed = .5;
}

6
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java
View File

@@ -20,11 +20,11 @@ import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstan
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kFeederSpeed;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kI;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kP;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kSFractional;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kSVolts;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterMotorPort;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterTargetRPS;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterToleranceRPS;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kVFractional;
import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kVVoltSecondsPerRotation;
public class ShooterSubsystem extends PIDSubsystem {
private final PWMVictorSPX m_shooterMotor = new PWMVictorSPX(kShooterMotorPort);
@@ -44,7 +44,7 @@ public class ShooterSubsystem extends PIDSubsystem {
@Override
public void useOutput(double output) {
// Use a feedforward of the form kS + kV * velocity
m_shooterMotor.set(output + kSFractional + kVFractional * kShooterTargetRPS);
m_shooterMotor.setVoltage(output + kSVolts + kVVoltSecondsPerRotation * kShooterTargetRPS);
}
@Override

6
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
View File

@@ -123,12 +123,12 @@ public class RobotContainer {
m_robotDrive.getRightEncoder()::getRate,
new PIDController(kPDriveVel, 0, 0),
new PIDController(kPDriveVel, 0, 0),
// RamseteCommand passes volts to the callback, so we have to rescale here
(left, right) -> m_robotDrive.tankDrive(left / 12., right / 12.),
// RamseteCommand passes volts to the callback
m_robotDrive::tankDriveVolts,
m_robotDrive
);
// Run path following command, then stop at the end.
return ramseteCommand.andThen(() -> m_robotDrive.tankDrive(0, 0));
return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVolts(0, 0));
}
}

12
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
View File

@@ -107,14 +107,14 @@ public class DriveSubsystem extends SubsystemBase {
}
/**
* Drives the robot using tank controls. Does not square inputs to enable composition with
* external controllers.
* Controls the left and right sides of the drive directly with voltages.
*
* @param left the commanded left output
* @param right the commanded right output
* @param leftVolts the commanded left output
* @param rightVolts the commanded right output
*/
public void tankDrive(double left, double right) {
m_drive.tankDrive(left, right, false);
public void tankDriveVolts(double leftVolts, double rightVolts) {
m_leftMotors.setVoltage(leftVolts);
m_rightMotors.setVoltage(-rightVolts);
}
/**