Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Add noise to Differential Drive simulator (#2903)

Browse Source 
Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com>
This commit is contained in:
Matt
2020-12-04 18:46:50 -08:00
committed by GitHub
parent 387f56cb7b
commit 963ad5c255
10 changed files with 253 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpiutil.math.VecBuilder;
public class DriveSubsystem extends SubsystemBase {
// The motors on the left side of the drive.
@@ -87,7 +88,8 @@ public class DriveSubsystem extends SubsystemBase {
Constants.DriveConstants.kDriveGearbox,
Constants.DriveConstants.kDriveGearing,
Constants.DriveConstants.kTrackwidthMeters,
Constants.DriveConstants.kWheelDiameterMeters / 2.0);
Constants.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
m_leftEncoderSim = new EncoderSim(m_leftEncoder);
@@ -118,10 +120,10 @@ public class DriveSubsystem extends SubsystemBase {
-m_rightMotors.get() * RobotController.getBatteryVoltage());
m_drivetrainSimulator.update(0.020);
m_leftEncoderSim.setDistance(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kLeftPosition));
m_leftEncoderSim.setRate(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kLeftVelocity));
m_rightEncoderSim.setDistance(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kRightPosition));
m_rightEncoderSim.setRate(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kRightVelocity));
m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters());
m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond());
m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters());
m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond());
m_gyroAngleSim.set(-m_drivetrainSimulator.getHeading().getDegrees());
m_fieldSim.setRobotPose(getPose());