Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'swerve' of https://github.com/Team4388/2022NoWayHome into swerve

Browse Source
This commit is contained in:
nathanrsxtn
2022-03-03 22:19:48 -07:00
parent c6041324d9 92d0e5a11c
commit d9b6eb4593
5 changed files with 36 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+14 -2
View File
@@ -147,14 +147,26 @@ public class SwerveDrive extends SubsystemBase {
public void periodic() {
updateOdometry();
// SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus));
updateSmartDash();
SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
// m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
super.periodic();
}
private void updateSmartDash() {
// odometry
SmartDashboard.putNumber("Odometry: X", getOdometry().getX());
SmartDashboard.putNumber("Odometry: Y", getOdometry().getY());
SmartDashboard.putNumber("Odometry: θ", getOdometry().getRotation().getDegrees());
// chassis speeds
// TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds
SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond);
SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond);
SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond);
}
/**
* Gets the distance between two given poses.
* @param p1 The first pose.