001// Copyright (c) FIRST and other WPILib contributors. 002// Open Source Software; you can modify and/or share it under the terms of 003// the WPILib BSD license file in the root directory of this project. 004 005package edu.wpi.first.math.kinematics; 006 007import static edu.wpi.first.units.Units.Meters; 008 009import edu.wpi.first.math.MathSharedStore; 010import edu.wpi.first.math.geometry.Pose2d; 011import edu.wpi.first.math.geometry.Rotation2d; 012import edu.wpi.first.units.measure.Distance; 013 014/** 015 * Class for differential drive odometry. Odometry allows you to track the robot's position on the 016 * field over the course of a match using readings from 2 encoders and a gyroscope. 017 * 018 * <p>Teams can use odometry during the autonomous period for complex tasks like path following. 019 * Furthermore, odometry can be used for latency compensation when using computer-vision systems. 020 * 021 * <p>It is important that you reset your encoders to zero before using this class. Any subsequent 022 * pose resets also require the encoders to be reset to zero. 023 */ 024public class DifferentialDriveOdometry extends Odometry<DifferentialDriveWheelPositions> { 025 /** 026 * Constructs a DifferentialDriveOdometry object. 027 * 028 * @param gyroAngle The angle reported by the gyroscope. 029 * @param leftDistance The distance traveled by the left encoder in meters. 030 * @param rightDistance The distance traveled by the right encoder in meters. 031 * @param initialPose The starting position of the robot on the field. 032 */ 033 public DifferentialDriveOdometry( 034 Rotation2d gyroAngle, double leftDistance, double rightDistance, Pose2d initialPose) { 035 super( 036 new DifferentialDriveKinematics(1), 037 gyroAngle, 038 new DifferentialDriveWheelPositions(leftDistance, rightDistance), 039 initialPose); 040 MathSharedStore.reportUsage("DifferentialDriveOdometry", ""); 041 } 042 043 /** 044 * Constructs a DifferentialDriveOdometry object. 045 * 046 * @param gyroAngle The angle reported by the gyroscope. 047 * @param leftDistance The distance traveled by the left encoder. 048 * @param rightDistance The distance traveled by the right encoder. 049 * @param initialPose The starting position of the robot on the field. 050 */ 051 public DifferentialDriveOdometry( 052 Rotation2d gyroAngle, Distance leftDistance, Distance rightDistance, Pose2d initialPose) { 053 this(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), initialPose); 054 } 055 056 /** 057 * Constructs a DifferentialDriveOdometry object. 058 * 059 * @param gyroAngle The angle reported by the gyroscope. 060 * @param leftDistance The distance traveled by the left encoder in meters. 061 * @param rightDistance The distance traveled by the right encoder in meters. 062 */ 063 public DifferentialDriveOdometry( 064 Rotation2d gyroAngle, double leftDistance, double rightDistance) { 065 this(gyroAngle, leftDistance, rightDistance, Pose2d.kZero); 066 } 067 068 /** 069 * Constructs a DifferentialDriveOdometry object. 070 * 071 * @param gyroAngle The angle reported by the gyroscope. 072 * @param leftDistance The distance traveled by the left encoder. 073 * @param rightDistance The distance traveled by the right encoder. 074 */ 075 public DifferentialDriveOdometry( 076 Rotation2d gyroAngle, Distance leftDistance, Distance rightDistance) { 077 this(gyroAngle, leftDistance, rightDistance, Pose2d.kZero); 078 } 079 080 /** 081 * Resets the robot's position on the field. 082 * 083 * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library 084 * automatically takes care of offsetting the gyro angle. 085 * 086 * @param gyroAngle The angle reported by the gyroscope. 087 * @param leftDistance The distance traveled by the left encoder in meters. 088 * @param rightDistance The distance traveled by the right encoder in meters. 089 * @param pose The position on the field that your robot is at. 090 */ 091 public void resetPosition( 092 Rotation2d gyroAngle, double leftDistance, double rightDistance, Pose2d pose) { 093 super.resetPosition( 094 gyroAngle, new DifferentialDriveWheelPositions(leftDistance, rightDistance), pose); 095 } 096 097 /** 098 * Resets the robot's position on the field. 099 * 100 * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library 101 * automatically takes care of offsetting the gyro angle. 102 * 103 * @param gyroAngle The angle reported by the gyroscope. 104 * @param leftDistance The distance traveled by the left encoder. 105 * @param rightDistance The distance traveled by the right encoder. 106 * @param pose The position on the field that your robot is at. 107 */ 108 public void resetPosition( 109 Rotation2d gyroAngle, Distance leftDistance, Distance rightDistance, Pose2d pose) { 110 resetPosition(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), pose); 111 } 112 113 /** 114 * Updates the robot position on the field using distance measurements from encoders. This method 115 * is more numerically accurate than using velocities to integrate the pose and is also 116 * advantageous for teams that are using lower CPR encoders. 117 * 118 * @param gyroAngle The angle reported by the gyroscope. 119 * @param leftDistance The distance traveled by the left encoder in meters. 120 * @param rightDistance The distance traveled by the right encoder in meters. 121 * @return The new pose of the robot. 122 */ 123 public Pose2d update(Rotation2d gyroAngle, double leftDistance, double rightDistance) { 124 return super.update( 125 gyroAngle, new DifferentialDriveWheelPositions(leftDistance, rightDistance)); 126 } 127}