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.MathUsageId; 011import edu.wpi.first.math.geometry.Twist2d; 012import edu.wpi.first.math.kinematics.proto.DifferentialDriveKinematicsProto; 013import edu.wpi.first.math.kinematics.struct.DifferentialDriveKinematicsStruct; 014import edu.wpi.first.units.Distance; 015import edu.wpi.first.units.Measure; 016 017/** 018 * Helper class that converts a chassis velocity (dx and dtheta components) to left and right wheel 019 * velocities for a differential drive. 020 * 021 * <p>Inverse kinematics converts a desired chassis speed into left and right velocity components 022 * whereas forward kinematics converts left and right component velocities into a linear and angular 023 * chassis speed. 024 */ 025public class DifferentialDriveKinematics 026 implements Kinematics<DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions> { 027 public final double trackWidthMeters; 028 029 public static final DifferentialDriveKinematicsProto proto = 030 new DifferentialDriveKinematicsProto(); 031 public static final DifferentialDriveKinematicsStruct struct = 032 new DifferentialDriveKinematicsStruct(); 033 034 /** 035 * Constructs a differential drive kinematics object. 036 * 037 * @param trackWidthMeters The track width of the drivetrain. Theoretically, this is the distance 038 * between the left wheels and right wheels. However, the empirical value may be larger than 039 * the physical measured value due to scrubbing effects. 040 */ 041 public DifferentialDriveKinematics(double trackWidthMeters) { 042 this.trackWidthMeters = trackWidthMeters; 043 MathSharedStore.reportUsage(MathUsageId.kKinematics_DifferentialDrive, 1); 044 } 045 046 /** 047 * Constructs a differential drive kinematics object. 048 * 049 * @param trackWidth The track width of the drivetrain. Theoretically, this is the distance 050 * between the left wheels and right wheels. However, the empirical value may be larger than 051 * the physical measured value due to scrubbing effects. 052 */ 053 public DifferentialDriveKinematics(Measure<Distance> trackWidth) { 054 this(trackWidth.in(Meters)); 055 } 056 057 /** 058 * Returns a chassis speed from left and right component velocities using forward kinematics. 059 * 060 * @param wheelSpeeds The left and right velocities. 061 * @return The chassis speed. 062 */ 063 @Override 064 public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) { 065 return new ChassisSpeeds( 066 (wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2, 067 0, 068 (wheelSpeeds.rightMetersPerSecond - wheelSpeeds.leftMetersPerSecond) / trackWidthMeters); 069 } 070 071 /** 072 * Returns left and right component velocities from a chassis speed using inverse kinematics. 073 * 074 * @param chassisSpeeds The linear and angular (dx and dtheta) components that represent the 075 * chassis' speed. 076 * @return The left and right velocities. 077 */ 078 @Override 079 public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) { 080 return new DifferentialDriveWheelSpeeds( 081 chassisSpeeds.vxMetersPerSecond 082 - trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond, 083 chassisSpeeds.vxMetersPerSecond 084 + trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond); 085 } 086 087 @Override 088 public Twist2d toTwist2d( 089 DifferentialDriveWheelPositions start, DifferentialDriveWheelPositions end) { 090 return toTwist2d(end.leftMeters - start.leftMeters, end.rightMeters - start.rightMeters); 091 } 092 093 /** 094 * Performs forward kinematics to return the resulting Twist2d from the given left and right side 095 * distance deltas. This method is often used for odometry -- determining the robot's position on 096 * the field using changes in the distance driven by each wheel on the robot. 097 * 098 * @param leftDistanceMeters The distance measured by the left side encoder. 099 * @param rightDistanceMeters The distance measured by the right side encoder. 100 * @return The resulting Twist2d. 101 */ 102 public Twist2d toTwist2d(double leftDistanceMeters, double rightDistanceMeters) { 103 return new Twist2d( 104 (leftDistanceMeters + rightDistanceMeters) / 2, 105 0, 106 (rightDistanceMeters - leftDistanceMeters) / trackWidthMeters); 107 } 108}