diff --git a/wpilibc/src/main/native/cpp/SpeedController.cpp b/wpilibc/src/main/native/cpp/SpeedController.cpp new file mode 100644 index 0000000000..cb3cc5b385 --- /dev/null +++ b/wpilibc/src/main/native/cpp/SpeedController.cpp @@ -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 + +using namespace frc; + +void SpeedController::SetVoltage(units::volt_t output) { + Set(output / units::volt_t(RobotController::GetInputVoltage())); +} diff --git a/wpilibc/src/main/native/include/frc/SpeedController.h b/wpilibc/src/main/native/include/frc/SpeedController.h index 49a828b37b..2b11aee1f7 100644 --- a/wpilibc/src/main/native/include/frc/SpeedController.h +++ b/wpilibc/src/main/native/include/frc/SpeedController.h @@ -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 + #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). + * + *

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. * diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp index d3633b6b4c..c0f9d28a8e 100644 --- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp @@ -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()); 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(); +} void ShooterSubsystem::RunFeeder() { m_feederMotor.Set(kFeederSpeed); } diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h index e74cfc0830..f9f07f4460 100644 --- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h @@ -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 diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp index c5d22d0244..7b2e7c4fb4 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp @@ -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); }, {})); } diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp index 3d5307f337..bc13cec27f 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp @@ -7,8 +7,6 @@ #include "subsystems/DriveSubsystem.h" -#include - #include #include @@ -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() { diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h index 620cfd88b8..86247d40a8 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h @@ -7,6 +7,8 @@ #pragma once +#include + #include #include #include @@ -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. diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedController.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedController.java index b90d19fe13..4407ff0eb1 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedController.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedController.java @@ -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). + * + *

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. * diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java index 384fb9892f..65b716e8ac 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java @@ -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; } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java index 5ebfb049b3..4b84f8178f 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java @@ -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 diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java index f8a3a25caa..75b4215525 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java @@ -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)); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java index 053fad97c5..7c51bf8370 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java @@ -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); } /**