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

odo auto

Browse Source
This commit is contained in:
aarav18
2022-02-05 11:50:49 -07:00
parent a7693d1c27
commit d49ee663fb
6 changed files with 25 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+17 -4
View File
@@ -14,6 +14,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
@@ -49,6 +50,7 @@ public class SwerveDrive extends SubsystemBase {
/* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used
below are robot specific, and should be tuned. */
public SwerveDrivePoseEstimator m_poseEstimator;
public SwerveDriveOdometry m_odometry;
public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
public boolean ignoreAngles;
@@ -88,9 +90,12 @@ public class SwerveDrive extends SubsystemBase {
m_gyro.getRotation2d(),
new Pose2d(),
m_kinematics,
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
VecBuilder.fill(Units.degreesToRadians(0.01)),
VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)),
VecBuilder.fill(Units.degreesToRadians(1)),
VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)));
m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
m_gyro.reset();
SmartDashboard.putData("Field", m_field);
}
@@ -150,7 +155,7 @@ public class SwerveDrive extends SubsystemBase {
@Override
public void periodic() {
//System.err.println(m_gyro.getFusedHeading() +" aaa");
// System.err.println(m_gyro.getFusedHeading() +" aaa");
updateOdometry();
SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus));
SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
@@ -197,6 +202,7 @@ public class SwerveDrive extends SubsystemBase {
* @return Robot's current pose.
*/
public Pose2d getOdometry() {
// return m_odometry.getPoseMeters();
return m_poseEstimator.getEstimatedPosition();
}
@@ -204,6 +210,7 @@ public class SwerveDrive extends SubsystemBase {
* Resets the odometry of the robot to (x=0, y=0, theta=0).
*/
public void resetOdometry(Pose2d pose) {
// m_odometry.resetPosition(pose, m_gyro.getRotation2d());
m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d());
}
@@ -216,6 +223,12 @@ public class SwerveDrive extends SubsystemBase {
modules[1].getState(),
modules[2].getState(),
modules[3].getState());
// m_odometry.update( m_gyro.getRotation2d(),
// modules[0].getState(),
// modules[1].getState(),
// modules[2].getState(),
// modules[3].getState());
// Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
// a real robot, this must be calculated based either on latency or timestamps.