mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
[wpiunits] Make Java units immutable only (#8115)
Remove mutable implementations, as systemcore doesn't need mutability to keep performance under control.
This commit is contained in:
@@ -8,9 +8,6 @@ import static edu.wpi.first.units.Units.Meters;
|
||||
import static edu.wpi.first.units.Units.MetersPerSecond;
|
||||
import static edu.wpi.first.units.Units.Volts;
|
||||
|
||||
import edu.wpi.first.units.measure.MutDistance;
|
||||
import edu.wpi.first.units.measure.MutLinearVelocity;
|
||||
import edu.wpi.first.units.measure.MutVoltage;
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.RobotController;
|
||||
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
|
||||
@@ -46,13 +43,6 @@ public class Drive extends SubsystemBase {
|
||||
DriveConstants.kRightEncoderPorts[1],
|
||||
DriveConstants.kRightEncoderReversed);
|
||||
|
||||
// Mutable holder for unit-safe voltage values, persisted to avoid reallocation.
|
||||
private final MutVoltage m_appliedVoltage = Volts.mutable(0);
|
||||
// Mutable holder for unit-safe linear distance values, persisted to avoid reallocation.
|
||||
private final MutDistance m_distance = Meters.mutable(0);
|
||||
// Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation.
|
||||
private final MutLinearVelocity m_velocity = MetersPerSecond.mutable(0);
|
||||
|
||||
// Create a new SysId routine for characterizing the drive.
|
||||
private final SysIdRoutine m_sysIdRoutine =
|
||||
new SysIdRoutine(
|
||||
@@ -70,21 +60,15 @@ public class Drive extends SubsystemBase {
|
||||
// Record a frame for the left motors. Since these share an encoder, we consider
|
||||
// the entire group to be one motor.
|
||||
log.motor("drive-left")
|
||||
.voltage(
|
||||
m_appliedVoltage.mut_replace(
|
||||
m_leftMotor.get() * RobotController.getBatteryVoltage(), Volts))
|
||||
.linearPosition(m_distance.mut_replace(m_leftEncoder.getDistance(), Meters))
|
||||
.linearVelocity(
|
||||
m_velocity.mut_replace(m_leftEncoder.getRate(), MetersPerSecond));
|
||||
.voltage(Volts.of(m_leftMotor.get() * RobotController.getBatteryVoltage()))
|
||||
.linearPosition(Meters.of(m_leftEncoder.getDistance()))
|
||||
.linearVelocity(MetersPerSecond.of(m_leftEncoder.getRate()));
|
||||
// Record a frame for the right motors. Since these share an encoder, we consider
|
||||
// the entire group to be one motor.
|
||||
log.motor("drive-right")
|
||||
.voltage(
|
||||
m_appliedVoltage.mut_replace(
|
||||
m_rightMotor.get() * RobotController.getBatteryVoltage(), Volts))
|
||||
.linearPosition(m_distance.mut_replace(m_rightEncoder.getDistance(), Meters))
|
||||
.linearVelocity(
|
||||
m_velocity.mut_replace(m_rightEncoder.getRate(), MetersPerSecond));
|
||||
.voltage(Volts.of(m_rightMotor.get() * RobotController.getBatteryVoltage()))
|
||||
.linearPosition(Meters.of(m_rightEncoder.getDistance()))
|
||||
.linearVelocity(MetersPerSecond.of(m_rightEncoder.getRate()));
|
||||
},
|
||||
// Tell SysId to make generated commands require this subsystem, suffix test state in
|
||||
// WPILog with this subsystem's name ("drive")
|
||||
|
||||
@@ -4,17 +4,12 @@
|
||||
|
||||
package edu.wpi.first.wpilibj.examples.sysidroutine.subsystems;
|
||||
|
||||
import static edu.wpi.first.units.Units.Radians;
|
||||
import static edu.wpi.first.units.Units.RadiansPerSecond;
|
||||
import static edu.wpi.first.units.Units.Rotations;
|
||||
import static edu.wpi.first.units.Units.RotationsPerSecond;
|
||||
import static edu.wpi.first.units.Units.Volts;
|
||||
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
|
||||
import edu.wpi.first.units.measure.MutAngle;
|
||||
import edu.wpi.first.units.measure.MutAngularVelocity;
|
||||
import edu.wpi.first.units.measure.MutVoltage;
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.RobotController;
|
||||
import edu.wpi.first.wpilibj.examples.sysidroutine.Constants.ShooterConstants;
|
||||
@@ -38,13 +33,6 @@ public class Shooter extends SubsystemBase {
|
||||
ShooterConstants.kEncoderPorts[1],
|
||||
ShooterConstants.kEncoderReversed);
|
||||
|
||||
// Mutable holder for unit-safe voltage values, persisted to avoid reallocation.
|
||||
private final MutVoltage m_appliedVoltage = Volts.mutable(0);
|
||||
// Mutable holder for unit-safe linear distance values, persisted to avoid reallocation.
|
||||
private final MutAngle m_angle = Radians.mutable(0);
|
||||
// Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation.
|
||||
private final MutAngularVelocity m_velocity = RadiansPerSecond.mutable(0);
|
||||
|
||||
// Create a new SysId routine for characterizing the shooter.
|
||||
private final SysIdRoutine m_sysIdRoutine =
|
||||
new SysIdRoutine(
|
||||
@@ -58,12 +46,9 @@ public class Shooter extends SubsystemBase {
|
||||
log -> {
|
||||
// Record a frame for the shooter motor.
|
||||
log.motor("shooter-wheel")
|
||||
.voltage(
|
||||
m_appliedVoltage.mut_replace(
|
||||
m_shooterMotor.get() * RobotController.getBatteryVoltage(), Volts))
|
||||
.angularPosition(m_angle.mut_replace(m_shooterEncoder.getDistance(), Rotations))
|
||||
.angularVelocity(
|
||||
m_velocity.mut_replace(m_shooterEncoder.getRate(), RotationsPerSecond));
|
||||
.voltage(Volts.of(m_shooterMotor.get() * RobotController.getBatteryVoltage()))
|
||||
.angularPosition(Rotations.of(m_shooterEncoder.getDistance()))
|
||||
.angularVelocity(RotationsPerSecond.of(m_shooterEncoder.getRate()));
|
||||
},
|
||||
// Tell SysId to make generated commands require this subsystem, suffix test state in
|
||||
// WPILog with this subsystem's name ("shooter")
|
||||
|
||||
Reference in New Issue
Block a user