mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
[wpilib] Document simulation APIs (#3079)
- Remove sim checkstyle suppression - Add [[nodiscard]] to C++ register callback functions - Add a couple of missing sim functions Co-authored-by: Peter Johnson <johnson.peter@gmail.com> Co-authored-by: Starlight220 <yotamshlomi@gmail.com>
This commit is contained in:
@@ -75,6 +75,7 @@ public class Drivetrain {
|
||||
new DifferentialDrivetrainSim(
|
||||
m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackWidth, kWheelRadius, null);
|
||||
|
||||
/** Subsystem constructor. */
|
||||
public Drivetrain() {
|
||||
// Set the distance per pulse for the drive encoders. We can simply use the
|
||||
// distance traveled for one rotation of the wheel divided by the encoder
|
||||
@@ -89,6 +90,7 @@ public class Drivetrain {
|
||||
SmartDashboard.putData("Field", m_fieldSim);
|
||||
}
|
||||
|
||||
/** Sets speeds to the drivetrain motors. */
|
||||
public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
|
||||
var leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
|
||||
var rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
|
||||
@@ -101,15 +103,24 @@ public class Drivetrain {
|
||||
m_rightGroup.setVoltage(rightOutput + rightFeedforward);
|
||||
}
|
||||
|
||||
/**
|
||||
* Controls the robot using arcade drive.
|
||||
*
|
||||
* @param xSpeed the speed for the x axis
|
||||
* @param rot the rotation
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public void drive(double xSpeed, double rot) {
|
||||
setSpeeds(m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0, rot)));
|
||||
}
|
||||
|
||||
/** Update robot odometry. */
|
||||
public void updateOdometry() {
|
||||
m_odometry.update(
|
||||
m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
|
||||
}
|
||||
|
||||
/** Resets robot odometry. */
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
m_leftEncoder.reset();
|
||||
m_rightEncoder.reset();
|
||||
@@ -117,10 +128,12 @@ public class Drivetrain {
|
||||
m_odometry.resetPosition(pose, m_gyro.getRotation2d());
|
||||
}
|
||||
|
||||
/** Check the current robot pose. */
|
||||
public Pose2d getPose() {
|
||||
return m_odometry.getPoseMeters();
|
||||
}
|
||||
|
||||
/** Update our simulation. This should be run every robot loop in simulation. */
|
||||
public void simulationPeriodic() {
|
||||
// To update our simulation, we set motor voltage inputs, update the
|
||||
// simulation, and write the simulated positions and velocities to our
|
||||
@@ -138,6 +151,7 @@ public class Drivetrain {
|
||||
m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees());
|
||||
}
|
||||
|
||||
/** Update odometry - this should be run every robot loop. */
|
||||
public void periodic() {
|
||||
updateOdometry();
|
||||
m_fieldSim.setRobotPose(m_odometry.getPoseMeters());
|
||||
|
||||
@@ -8,6 +8,7 @@ import static edu.wpi.first.wpilibj.XboxController.Button;
|
||||
|
||||
import edu.wpi.first.wpilibj.GenericHID;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
import edu.wpi.first.wpilibj.XboxController.Button;
|
||||
import edu.wpi.first.wpilibj.controller.PIDController;
|
||||
import edu.wpi.first.wpilibj.controller.RamseteController;
|
||||
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
|
||||
|
||||
@@ -11,7 +11,7 @@ import edu.wpi.first.wpilibj.RobotBase;
|
||||
import edu.wpi.first.wpilibj.RobotController;
|
||||
import edu.wpi.first.wpilibj.SpeedControllerGroup;
|
||||
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
|
||||
import edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation.Constants;
|
||||
import edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation.Constants.DriveConstants;
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
|
||||
@@ -28,14 +28,14 @@ public class DriveSubsystem extends SubsystemBase {
|
||||
// The motors on the left side of the drive.
|
||||
private final SpeedControllerGroup m_leftMotors =
|
||||
new SpeedControllerGroup(
|
||||
new PWMVictorSPX(Constants.DriveConstants.kLeftMotor1Port),
|
||||
new PWMVictorSPX(Constants.DriveConstants.kLeftMotor2Port));
|
||||
new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
|
||||
new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
|
||||
|
||||
// The motors on the right side of the drive.
|
||||
private final SpeedControllerGroup m_rightMotors =
|
||||
new SpeedControllerGroup(
|
||||
new PWMVictorSPX(Constants.DriveConstants.kRightMotor1Port),
|
||||
new PWMVictorSPX(Constants.DriveConstants.kRightMotor2Port));
|
||||
new PWMVictorSPX(DriveConstants.kRightMotor1Port),
|
||||
new PWMVictorSPX(DriveConstants.kRightMotor2Port));
|
||||
|
||||
// The robot's drive
|
||||
private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
|
||||
@@ -43,16 +43,16 @@ public class DriveSubsystem extends SubsystemBase {
|
||||
// The left-side drive encoder
|
||||
private final Encoder m_leftEncoder =
|
||||
new Encoder(
|
||||
Constants.DriveConstants.kLeftEncoderPorts[0],
|
||||
Constants.DriveConstants.kLeftEncoderPorts[1],
|
||||
Constants.DriveConstants.kLeftEncoderReversed);
|
||||
DriveConstants.kLeftEncoderPorts[0],
|
||||
DriveConstants.kLeftEncoderPorts[1],
|
||||
DriveConstants.kLeftEncoderReversed);
|
||||
|
||||
// The right-side drive encoder
|
||||
private final Encoder m_rightEncoder =
|
||||
new Encoder(
|
||||
Constants.DriveConstants.kRightEncoderPorts[0],
|
||||
Constants.DriveConstants.kRightEncoderPorts[1],
|
||||
Constants.DriveConstants.kRightEncoderReversed);
|
||||
DriveConstants.kRightEncoderPorts[0],
|
||||
DriveConstants.kRightEncoderPorts[1],
|
||||
DriveConstants.kRightEncoderReversed);
|
||||
|
||||
// The gyro sensor
|
||||
private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro();
|
||||
@@ -71,8 +71,8 @@ public class DriveSubsystem extends SubsystemBase {
|
||||
/** Creates a new DriveSubsystem. */
|
||||
public DriveSubsystem() {
|
||||
// Sets the distance per pulse for the encoders
|
||||
m_leftEncoder.setDistancePerPulse(Constants.DriveConstants.kEncoderDistancePerPulse);
|
||||
m_rightEncoder.setDistancePerPulse(Constants.DriveConstants.kEncoderDistancePerPulse);
|
||||
m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
|
||||
m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
|
||||
|
||||
resetEncoders();
|
||||
m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()));
|
||||
@@ -81,11 +81,11 @@ public class DriveSubsystem extends SubsystemBase {
|
||||
// This class simulates our drivetrain's motion around the field.
|
||||
m_drivetrainSimulator =
|
||||
new DifferentialDrivetrainSim(
|
||||
Constants.DriveConstants.kDrivetrainPlant,
|
||||
Constants.DriveConstants.kDriveGearbox,
|
||||
Constants.DriveConstants.kDriveGearing,
|
||||
Constants.DriveConstants.kTrackwidthMeters,
|
||||
Constants.DriveConstants.kWheelDiameterMeters / 2.0,
|
||||
DriveConstants.kDrivetrainPlant,
|
||||
DriveConstants.kDriveGearbox,
|
||||
DriveConstants.kDriveGearing,
|
||||
DriveConstants.kTrackwidthMeters,
|
||||
DriveConstants.kWheelDiameterMeters / 2.0,
|
||||
VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005));
|
||||
|
||||
// The encoder and gyro angle sims let us set simulated sensor readings
|
||||
@@ -246,7 +246,6 @@ public class DriveSubsystem extends SubsystemBase {
|
||||
* @return the robot's heading in degrees, from -180 to 180
|
||||
*/
|
||||
public double getHeading() {
|
||||
return Math.IEEEremainder(m_gyro.getAngle(), 360)
|
||||
* (Constants.DriveConstants.kGyroReversed ? -1.0 : 1.0);
|
||||
return Math.IEEEremainder(m_gyro.getAngle(), 360) * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user