mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
Replace .to<double>() and .template to<double>() with .value() (#3667)
It's a less verbose way to do the same thing.
This commit is contained in:
@@ -16,7 +16,7 @@ ArmSubsystem::ArmSubsystem()
|
||||
m_motor(kMotorPort),
|
||||
m_encoder(kEncoderPorts[0], kEncoderPorts[1]),
|
||||
m_feedforward(kS, kCos, kV, kA) {
|
||||
m_encoder.SetDistancePerPulse(kEncoderDistancePerPulse.to<double>());
|
||||
m_encoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
|
||||
// Start arm in neutral position
|
||||
SetGoal(State{kArmOffset, 0_rad_per_s});
|
||||
}
|
||||
|
||||
@@ -23,5 +23,5 @@ void ArmSubsystem::UseState(State setpoint) {
|
||||
m_feedforward.Calculate(setpoint.position, setpoint.velocity);
|
||||
// Add the feedforward to the PID output to get the motor output
|
||||
m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
|
||||
setpoint.position.to<double>(), feedforward / 12_V);
|
||||
setpoint.position.value(), feedforward / 12_V);
|
||||
}
|
||||
|
||||
@@ -94,7 +94,7 @@ class Robot : public frc::TimedRobot {
|
||||
|
||||
// Finally, we set our simulated encoder's readings and simulated battery
|
||||
// voltage
|
||||
m_encoderSim.SetDistance(m_armSim.GetAngle().to<double>());
|
||||
m_encoderSim.SetDistance(m_armSim.GetAngle().value());
|
||||
// SimBattery estimates loaded battery voltages
|
||||
frc::sim::RoboRioSim::SetVInVoltage(
|
||||
frc::sim::BatterySim::Calculate({m_armSim.GetCurrentDraw()}));
|
||||
@@ -108,7 +108,7 @@ class Robot : public frc::TimedRobot {
|
||||
// Here, we run PID control like normal, with a constant setpoint of 75
|
||||
// degrees.
|
||||
double pidOutput = m_controller.Calculate(
|
||||
m_encoder.GetDistance(), (units::radian_t(75_deg)).to<double>());
|
||||
m_encoder.GetDistance(), (units::radian_t(75_deg)).value());
|
||||
m_motor.SetVoltage(units::volt_t(pidOutput));
|
||||
} else {
|
||||
// Otherwise, we disable the motor.
|
||||
|
||||
@@ -8,9 +8,9 @@ void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
|
||||
const auto leftFeedforward = m_feedforward.Calculate(speeds.left);
|
||||
const auto rightFeedforward = m_feedforward.Calculate(speeds.right);
|
||||
const double leftOutput = m_leftPIDController.Calculate(
|
||||
m_leftEncoder.GetRate(), speeds.left.to<double>());
|
||||
m_leftEncoder.GetRate(), speeds.left.value());
|
||||
const double rightOutput = m_rightPIDController.Calculate(
|
||||
m_rightEncoder.GetRate(), speeds.right.to<double>());
|
||||
m_rightEncoder.GetRate(), speeds.right.value());
|
||||
|
||||
m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
|
||||
m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
|
||||
|
||||
@@ -12,9 +12,9 @@ void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
|
||||
const auto leftFeedforward = m_feedforward.Calculate(speeds.left);
|
||||
const auto rightFeedforward = m_feedforward.Calculate(speeds.right);
|
||||
const double leftOutput = m_leftPIDController.Calculate(
|
||||
m_leftEncoder.GetRate(), speeds.left.to<double>());
|
||||
m_leftEncoder.GetRate(), speeds.left.value());
|
||||
const double rightOutput = m_rightPIDController.Calculate(
|
||||
m_rightEncoder.GetRate(), speeds.right.to<double>());
|
||||
m_rightEncoder.GetRate(), speeds.right.value());
|
||||
|
||||
m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
|
||||
m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
|
||||
|
||||
@@ -34,9 +34,9 @@ class Drivetrain {
|
||||
// distance traveled for one rotation of the wheel divided by the encoder
|
||||
// resolution.
|
||||
m_leftEncoder.SetDistancePerPulse(
|
||||
2 * wpi::numbers::pi * kWheelRadius.to<double>() / kEncoderResolution);
|
||||
2 * wpi::numbers::pi * kWheelRadius.value() / kEncoderResolution);
|
||||
m_rightEncoder.SetDistancePerPulse(
|
||||
2 * wpi::numbers::pi * kWheelRadius.to<double>() / kEncoderResolution);
|
||||
2 * wpi::numbers::pi * kWheelRadius.value() / kEncoderResolution);
|
||||
|
||||
m_leftEncoder.Reset();
|
||||
m_rightEncoder.Reset();
|
||||
|
||||
@@ -37,10 +37,10 @@ void DriveSubsystem::SetDriveStates(
|
||||
frc::TrapezoidProfile<units::meters>::State left,
|
||||
frc::TrapezoidProfile<units::meters>::State right) {
|
||||
m_leftLeader.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
|
||||
left.position.to<double>(),
|
||||
left.position.value(),
|
||||
m_feedforward.Calculate(left.velocity) / 12_V);
|
||||
m_rightLeader.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
|
||||
right.position.to<double>(),
|
||||
right.position.value(),
|
||||
m_feedforward.Calculate(right.velocity) / 12_V);
|
||||
}
|
||||
|
||||
|
||||
@@ -31,7 +31,7 @@ class Robot : public frc::TimedRobot {
|
||||
|
||||
frc::SmartDashboard::PutBoolean("Connected", connected);
|
||||
frc::SmartDashboard::PutNumber("Frequency", frequency);
|
||||
frc::SmartDashboard::PutNumber("Output", output.to<double>());
|
||||
frc::SmartDashboard::PutNumber("Output", output.value());
|
||||
frc::SmartDashboard::PutNumber("Distance", distance);
|
||||
}
|
||||
};
|
||||
|
||||
@@ -46,7 +46,7 @@ class Robot : public frc::TimedRobot {
|
||||
// distance per pulse = (distance per revolution) / (pulses per revolution)
|
||||
// = (Pi * D) / ppr
|
||||
static constexpr double kArmEncoderDistPerPulse =
|
||||
2.0 * wpi::numbers::pi * kElevatorDrumRadius.to<double>() / 4096.0;
|
||||
2.0 * wpi::numbers::pi * kElevatorDrumRadius.value() / 4096.0;
|
||||
|
||||
// This gearbox represents a gearbox containing 4 Vex 775pro motors.
|
||||
frc::DCMotor m_elevatorGearbox = frc::DCMotor::Vex775Pro(4);
|
||||
@@ -73,7 +73,7 @@ class Robot : public frc::TimedRobot {
|
||||
m_mech2d.GetRoot("Elevator Root", 10, 0);
|
||||
frc::MechanismLigament2d* m_elevatorMech2d =
|
||||
m_elevatorRoot->Append<frc::MechanismLigament2d>(
|
||||
"Elevator", units::inch_t(m_elevatorSim.GetPosition()).to<double>(),
|
||||
"Elevator", units::inch_t(m_elevatorSim.GetPosition()).value(),
|
||||
90_deg);
|
||||
|
||||
public:
|
||||
@@ -95,21 +95,21 @@ class Robot : public frc::TimedRobot {
|
||||
|
||||
// Finally, we set our simulated encoder's readings and simulated battery
|
||||
// voltage
|
||||
m_encoderSim.SetDistance(m_elevatorSim.GetPosition().to<double>());
|
||||
m_encoderSim.SetDistance(m_elevatorSim.GetPosition().value());
|
||||
// SimBattery estimates loaded battery voltages
|
||||
frc::sim::RoboRioSim::SetVInVoltage(
|
||||
frc::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()}));
|
||||
|
||||
// Update the Elevator length based on the simulated elevator height
|
||||
m_elevatorMech2d->SetLength(
|
||||
units::inch_t(m_elevatorSim.GetPosition()).to<double>());
|
||||
units::inch_t(m_elevatorSim.GetPosition()).value());
|
||||
}
|
||||
|
||||
void TeleopPeriodic() override {
|
||||
if (m_joystick.GetTrigger()) {
|
||||
// Here, we run PID control like normal, with a constant setpoint of 30in.
|
||||
double pidOutput = m_controller.Calculate(
|
||||
m_encoder.GetDistance(), units::meter_t(30_in).to<double>());
|
||||
double pidOutput = m_controller.Calculate(m_encoder.GetDistance(),
|
||||
units::meter_t(30_in).value());
|
||||
m_motor.SetVoltage(units::volt_t(pidOutput));
|
||||
} else {
|
||||
// Otherwise, we disable the motor.
|
||||
|
||||
@@ -43,7 +43,7 @@ class Robot : public frc::TimedRobot {
|
||||
|
||||
// Send setpoint to offboard controller PID
|
||||
m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
|
||||
m_setpoint.position.to<double>(),
|
||||
m_setpoint.position.value(),
|
||||
m_feedforward.Calculate(m_setpoint.velocity) / 12_V);
|
||||
}
|
||||
|
||||
|
||||
@@ -16,9 +16,9 @@ ShooterSubsystem::ShooterSubsystem()
|
||||
m_feederMotor(kFeederMotorPort),
|
||||
m_shooterEncoder(kEncoderPorts[0], kEncoderPorts[1]),
|
||||
m_shooterFeedforward(kS, kV) {
|
||||
m_controller.SetTolerance(kShooterToleranceRPS.to<double>());
|
||||
m_controller.SetTolerance(kShooterToleranceRPS.value());
|
||||
m_shooterEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
|
||||
SetSetpoint(kShooterTargetRPS.to<double>());
|
||||
SetSetpoint(kShooterTargetRPS.value());
|
||||
}
|
||||
|
||||
void ShooterSubsystem::UseOutput(double output, double setpoint) {
|
||||
|
||||
@@ -25,9 +25,9 @@ Drivetrain::Drivetrain() {
|
||||
m_rightEncoder.SetDistancePerPulse(0.042);
|
||||
#else
|
||||
// Circumference = diameter * pi. 360 tick simulated encoders.
|
||||
m_leftEncoder.SetDistancePerPulse(units::foot_t{4_in}.to<double>() *
|
||||
m_leftEncoder.SetDistancePerPulse(units::foot_t{4_in}.value() *
|
||||
wpi::numbers::pi / 360.0);
|
||||
m_rightEncoder.SetDistancePerPulse(units::foot_t{4_in}.to<double>() *
|
||||
m_rightEncoder.SetDistancePerPulse(units::foot_t{4_in}.value() *
|
||||
wpi::numbers::pi / 360.0);
|
||||
#endif
|
||||
SetName("Drivetrain");
|
||||
|
||||
@@ -12,9 +12,9 @@ TurnToAngle::TurnToAngle(units::degree_t target, DriveSubsystem* drive)
|
||||
: CommandHelper(
|
||||
frc2::PIDController(kTurnP, kTurnI, kTurnD),
|
||||
// Close loop on heading
|
||||
[drive] { return drive->GetHeading().to<double>(); },
|
||||
[drive] { return drive->GetHeading().value(); },
|
||||
// Set reference to target
|
||||
target.to<double>(),
|
||||
target.value(),
|
||||
// Pipe output to turn robot
|
||||
[drive](double output) { drive->ArcadeDrive(0, output); },
|
||||
// Require the drive
|
||||
@@ -24,8 +24,7 @@ TurnToAngle::TurnToAngle(units::degree_t target, DriveSubsystem* drive)
|
||||
// Set the controller tolerance - the delta tolerance ensures the robot is
|
||||
// stationary at the setpoint before it is considered as having reached the
|
||||
// reference
|
||||
m_controller.SetTolerance(kTurnTolerance.to<double>(),
|
||||
kTurnRateTolerance.to<double>());
|
||||
m_controller.SetTolerance(kTurnTolerance.value(), kTurnRateTolerance.value());
|
||||
|
||||
AddRequirements({drive});
|
||||
}
|
||||
|
||||
@@ -22,13 +22,13 @@ void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) {
|
||||
m_feedforward.Calculate(wheelSpeeds.rearRight);
|
||||
|
||||
const double frontLeftOutput = m_frontLeftPIDController.Calculate(
|
||||
m_frontLeftEncoder.GetRate(), wheelSpeeds.frontLeft.to<double>());
|
||||
m_frontLeftEncoder.GetRate(), wheelSpeeds.frontLeft.value());
|
||||
const double frontRightOutput = m_frontRightPIDController.Calculate(
|
||||
m_frontRightEncoder.GetRate(), wheelSpeeds.frontRight.to<double>());
|
||||
m_frontRightEncoder.GetRate(), wheelSpeeds.frontRight.value());
|
||||
const double backLeftOutput = m_backLeftPIDController.Calculate(
|
||||
m_backLeftEncoder.GetRate(), wheelSpeeds.rearLeft.to<double>());
|
||||
m_backLeftEncoder.GetRate(), wheelSpeeds.rearLeft.value());
|
||||
const double backRightOutput = m_backRightPIDController.Calculate(
|
||||
m_backRightEncoder.GetRate(), wheelSpeeds.rearRight.to<double>());
|
||||
m_backRightEncoder.GetRate(), wheelSpeeds.rearRight.value());
|
||||
|
||||
m_frontLeftMotor.SetVoltage(units::volt_t{frontLeftOutput} +
|
||||
frontLeftFeedforward);
|
||||
|
||||
@@ -18,15 +18,14 @@ frc::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const {
|
||||
void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) {
|
||||
std::function<void(units::meters_per_second_t, const frc::Encoder&,
|
||||
frc2::PIDController&, frc::PWMSparkMax&)>
|
||||
calcAndSetSpeeds =
|
||||
[&m_feedforward = m_feedforward](units::meters_per_second_t speed,
|
||||
const auto& encoder,
|
||||
auto& controller, auto& motor) {
|
||||
auto feedforward = m_feedforward.Calculate(speed);
|
||||
double output =
|
||||
controller.Calculate(encoder.GetRate(), speed.to<double>());
|
||||
motor.SetVoltage(units::volt_t{output} + feedforward);
|
||||
};
|
||||
calcAndSetSpeeds = [&m_feedforward = m_feedforward](
|
||||
units::meters_per_second_t speed,
|
||||
const auto& encoder, auto& controller,
|
||||
auto& motor) {
|
||||
auto feedforward = m_feedforward.Calculate(speed);
|
||||
double output = controller.Calculate(encoder.GetRate(), speed.value());
|
||||
motor.SetVoltage(units::volt_t{output} + feedforward);
|
||||
};
|
||||
|
||||
calcAndSetSpeeds(wheelSpeeds.frontLeft, m_frontLeftEncoder,
|
||||
m_frontLeftPIDController, m_frontLeftMotor);
|
||||
|
||||
@@ -8,9 +8,9 @@ void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
|
||||
const auto leftFeedforward = m_feedforward.Calculate(speeds.left);
|
||||
const auto rightFeedforward = m_feedforward.Calculate(speeds.right);
|
||||
const double leftOutput = m_leftPIDController.Calculate(
|
||||
m_leftEncoder.GetRate(), speeds.left.to<double>());
|
||||
m_leftEncoder.GetRate(), speeds.left.value());
|
||||
const double rightOutput = m_rightPIDController.Calculate(
|
||||
m_rightEncoder.GetRate(), speeds.right.to<double>());
|
||||
m_rightEncoder.GetRate(), speeds.right.value());
|
||||
|
||||
m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
|
||||
m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
|
||||
|
||||
@@ -20,10 +20,10 @@ Drivetrain::Drivetrain() {
|
||||
// gearbox is constructed, you might have to invert the left side instead.
|
||||
m_rightMotor.SetInverted(true);
|
||||
|
||||
m_leftEncoder.SetDistancePerPulse(
|
||||
wpi::numbers::pi * kWheelDiameter.to<double>() / kCountsPerRevolution);
|
||||
m_rightEncoder.SetDistancePerPulse(
|
||||
wpi::numbers::pi * kWheelDiameter.to<double>() / kCountsPerRevolution);
|
||||
m_leftEncoder.SetDistancePerPulse(wpi::numbers::pi * kWheelDiameter.value() /
|
||||
kCountsPerRevolution);
|
||||
m_rightEncoder.SetDistancePerPulse(wpi::numbers::pi * kWheelDiameter.value() /
|
||||
kCountsPerRevolution);
|
||||
ResetEncoders();
|
||||
}
|
||||
|
||||
|
||||
@@ -10,9 +10,9 @@ void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
|
||||
auto leftFeedforward = m_feedforward.Calculate(speeds.left);
|
||||
auto rightFeedforward = m_feedforward.Calculate(speeds.right);
|
||||
double leftOutput = m_leftPIDController.Calculate(m_leftEncoder.GetRate(),
|
||||
speeds.left.to<double>());
|
||||
double rightOutput = m_rightPIDController.Calculate(
|
||||
m_rightEncoder.GetRate(), speeds.right.to<double>());
|
||||
speeds.left.value());
|
||||
double rightOutput = m_rightPIDController.Calculate(m_rightEncoder.GetRate(),
|
||||
speeds.right.value());
|
||||
|
||||
m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
|
||||
m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
|
||||
@@ -47,16 +47,12 @@ void Drivetrain::SimulationPeriodic() {
|
||||
frc::RobotController::GetInputVoltage());
|
||||
m_drivetrainSimulator.Update(20_ms);
|
||||
|
||||
m_leftEncoderSim.SetDistance(
|
||||
m_drivetrainSimulator.GetLeftPosition().to<double>());
|
||||
m_leftEncoderSim.SetRate(
|
||||
m_drivetrainSimulator.GetLeftVelocity().to<double>());
|
||||
m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetLeftPosition().value());
|
||||
m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetLeftVelocity().value());
|
||||
m_rightEncoderSim.SetDistance(
|
||||
m_drivetrainSimulator.GetRightPosition().to<double>());
|
||||
m_rightEncoderSim.SetRate(
|
||||
m_drivetrainSimulator.GetRightVelocity().to<double>());
|
||||
m_gyroSim.SetAngle(
|
||||
-m_drivetrainSimulator.GetHeading().Degrees().to<double>());
|
||||
m_drivetrainSimulator.GetRightPosition().value());
|
||||
m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetRightVelocity().value());
|
||||
m_gyroSim.SetAngle(-m_drivetrainSimulator.GetHeading().Degrees().value());
|
||||
}
|
||||
|
||||
void Drivetrain::Periodic() {
|
||||
|
||||
@@ -122,9 +122,9 @@ class Robot : public frc::TimedRobot {
|
||||
m_lastProfiledReference))
|
||||
.Calculate(20_ms);
|
||||
|
||||
m_loop.SetNextR(Eigen::Vector<double, 2>{
|
||||
m_lastProfiledReference.position.to<double>(),
|
||||
m_lastProfiledReference.velocity.to<double>()});
|
||||
m_loop.SetNextR(
|
||||
Eigen::Vector<double, 2>{m_lastProfiledReference.position.value(),
|
||||
m_lastProfiledReference.velocity.value()});
|
||||
|
||||
// Correct our Kalman filter's state vector estimate with encoder data.
|
||||
m_loop.Correct(Eigen::Vector<double, 1>{m_encoder.GetDistance()});
|
||||
|
||||
@@ -46,14 +46,11 @@ void DriveSubsystem::SimulationPeriodic() {
|
||||
frc::RobotController::GetInputVoltage());
|
||||
m_drivetrainSimulator.Update(20_ms);
|
||||
|
||||
m_leftEncoderSim.SetDistance(
|
||||
m_drivetrainSimulator.GetLeftPosition().to<double>());
|
||||
m_leftEncoderSim.SetRate(
|
||||
m_drivetrainSimulator.GetLeftVelocity().to<double>());
|
||||
m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetLeftPosition().value());
|
||||
m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetLeftVelocity().value());
|
||||
m_rightEncoderSim.SetDistance(
|
||||
m_drivetrainSimulator.GetRightPosition().to<double>());
|
||||
m_rightEncoderSim.SetRate(
|
||||
m_drivetrainSimulator.GetRightVelocity().to<double>());
|
||||
m_drivetrainSimulator.GetRightPosition().value());
|
||||
m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetRightVelocity().value());
|
||||
m_gyroSim.SetAngle(-m_drivetrainSimulator.GetHeading().Degrees());
|
||||
}
|
||||
|
||||
|
||||
@@ -43,7 +43,7 @@ constexpr int kEncoderCPR = 1024;
|
||||
constexpr auto kWheelDiameter = 6_in;
|
||||
constexpr double kEncoderDistancePerPulse =
|
||||
// Assumes the encoders are directly mounted on the wheel shafts
|
||||
(kWheelDiameter.to<double>() * wpi::numbers::pi) /
|
||||
(kWheelDiameter.value() * wpi::numbers::pi) /
|
||||
static_cast<double>(kEncoderCPR);
|
||||
|
||||
// These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
|
||||
|
||||
@@ -90,8 +90,8 @@ class Robot : public frc::TimedRobot {
|
||||
public:
|
||||
void RobotInit() override {
|
||||
// Circumference = pi * d, so distance per click = pi * d / counts
|
||||
m_encoder.SetDistancePerPulse(2.0 * wpi::numbers::pi *
|
||||
kDrumRadius.to<double>() / 4096.0);
|
||||
m_encoder.SetDistancePerPulse(2.0 * wpi::numbers::pi * kDrumRadius.value() /
|
||||
4096.0);
|
||||
}
|
||||
|
||||
void TeleopInit() override {
|
||||
@@ -119,9 +119,9 @@ class Robot : public frc::TimedRobot {
|
||||
m_lastProfiledReference))
|
||||
.Calculate(20_ms);
|
||||
|
||||
m_loop.SetNextR(Eigen::Vector<double, 2>{
|
||||
m_lastProfiledReference.position.to<double>(),
|
||||
m_lastProfiledReference.velocity.to<double>()});
|
||||
m_loop.SetNextR(
|
||||
Eigen::Vector<double, 2>{m_lastProfiledReference.position.value(),
|
||||
m_lastProfiledReference.velocity.value()});
|
||||
|
||||
// Correct our Kalman filter's state vector estimate with encoder data.
|
||||
m_loop.Correct(Eigen::Vector<double, 1>{m_encoder.GetDistance()});
|
||||
|
||||
@@ -93,7 +93,7 @@ class Robot : public frc::TimedRobot {
|
||||
// setpoint of a PID controller.
|
||||
if (m_joystick.GetRightBumper()) {
|
||||
// We pressed the bumper, so let's set our next reference
|
||||
m_loop.SetNextR(Eigen::Vector<double, 1>{kSpinup.to<double>()});
|
||||
m_loop.SetNextR(Eigen::Vector<double, 1>{kSpinup.value()});
|
||||
} else {
|
||||
// We released the bumper, so let's spin down
|
||||
m_loop.SetNextR(Eigen::Vector<double, 1>{0.0});
|
||||
|
||||
@@ -94,7 +94,7 @@ class Robot : public frc::TimedRobot {
|
||||
// setpoint of a PID controller.
|
||||
if (m_joystick.GetRightBumper()) {
|
||||
// We pressed the bumper, so let's set our next reference
|
||||
m_loop.SetNextR(Eigen::Vector<double, 1>{kSpinup.to<double>()});
|
||||
m_loop.SetNextR(Eigen::Vector<double, 1>{kSpinup.value()});
|
||||
} else {
|
||||
// We released the bumper, so let's spin down
|
||||
m_loop.SetNextR(Eigen::Vector<double, 1>{0.0});
|
||||
|
||||
@@ -48,7 +48,7 @@ void SwerveModule::SetDesiredState(
|
||||
|
||||
// Calculate the drive output from the drive PID controller.
|
||||
const auto driveOutput = m_drivePIDController.Calculate(
|
||||
m_driveEncoder.GetRate(), state.speed.to<double>());
|
||||
m_driveEncoder.GetRate(), state.speed.value());
|
||||
|
||||
const auto driveFeedforward = m_driveFeedforward.Calculate(state.speed);
|
||||
|
||||
|
||||
@@ -51,7 +51,7 @@ void SwerveModule::SetDesiredState(
|
||||
|
||||
// Calculate the drive output from the drive PID controller.
|
||||
const auto driveOutput = m_drivePIDController.Calculate(
|
||||
m_driveEncoder.GetRate(), state.speed.to<double>());
|
||||
m_driveEncoder.GetRate(), state.speed.value());
|
||||
|
||||
// Calculate the turning motor output from the turning PID controller.
|
||||
auto turnOutput = m_turningPIDController.Calculate(
|
||||
|
||||
@@ -20,8 +20,8 @@ SwerveModule::SwerveModule(const int driveMotorChannel,
|
||||
// Set the distance per pulse for the drive encoder. We can simply use the
|
||||
// distance traveled for one rotation of the wheel divided by the encoder
|
||||
// resolution.
|
||||
m_driveEncoder.SetDistancePerPulse(
|
||||
2 * wpi::numbers::pi * kWheelRadius.to<double>() / kEncoderResolution);
|
||||
m_driveEncoder.SetDistancePerPulse(2 * wpi::numbers::pi *
|
||||
kWheelRadius.value() / kEncoderResolution);
|
||||
|
||||
// Set the distance (in this case, angle) per pulse for the turning encoder.
|
||||
// This is the the angle through an entire rotation (2 * wpi::numbers::pi)
|
||||
@@ -48,7 +48,7 @@ void SwerveModule::SetDesiredState(
|
||||
|
||||
// Calculate the drive output from the drive PID controller.
|
||||
const auto driveOutput = m_drivePIDController.Calculate(
|
||||
m_driveEncoder.GetRate(), state.speed.to<double>());
|
||||
m_driveEncoder.GetRate(), state.speed.value());
|
||||
|
||||
const auto driveFeedforward = m_driveFeedforward.Calculate(state.speed);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user