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.Twist2d; 011import edu.wpi.first.math.kinematics.proto.DifferentialDriveKinematicsProto; 012import edu.wpi.first.math.kinematics.struct.DifferentialDriveKinematicsStruct; 013import edu.wpi.first.units.measure.Distance; 014import edu.wpi.first.util.protobuf.ProtobufSerializable; 015import edu.wpi.first.util.struct.StructSerializable; 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 ProtobufSerializable, 028 StructSerializable { 029 /** Differential drive trackwidth in meters. */ 030 public final double trackwidth; 031 032 /** DifferentialDriveKinematics protobuf for serialization. */ 033 public static final DifferentialDriveKinematicsProto proto = 034 new DifferentialDriveKinematicsProto(); 035 036 /** DifferentialDriveKinematics struct for serialization. */ 037 public static final DifferentialDriveKinematicsStruct struct = 038 new DifferentialDriveKinematicsStruct(); 039 040 /** 041 * Constructs a differential drive kinematics object. 042 * 043 * @param trackwidth The trackwidth of the drivetrain in meters. Theoretically, this is the 044 * distance between the left wheels and right wheels. However, the empirical value may be 045 * larger than the physical measured value due to scrubbing effects. 046 */ 047 public DifferentialDriveKinematics(double trackwidth) { 048 this.trackwidth = trackwidth; 049 MathSharedStore.reportUsage("DifferentialDriveKinematics", ""); 050 } 051 052 /** 053 * Constructs a differential drive kinematics object. 054 * 055 * @param trackwidth The trackwidth of the drivetrain in meters. Theoretically, this is the 056 * distance between the left wheels and right wheels. However, the empirical value may be 057 * larger than the physical measured value due to scrubbing effects. 058 */ 059 public DifferentialDriveKinematics(Distance trackwidth) { 060 this(trackwidth.in(Meters)); 061 } 062 063 /** 064 * Returns a chassis speed from left and right component velocities using forward kinematics. 065 * 066 * @param wheelSpeeds The left and right velocities. 067 * @return The chassis speed. 068 */ 069 @Override 070 public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) { 071 return new ChassisSpeeds( 072 (wheelSpeeds.left + wheelSpeeds.right) / 2, 073 0, 074 (wheelSpeeds.right - wheelSpeeds.left) / trackwidth); 075 } 076 077 /** 078 * Returns left and right component velocities from a chassis speed using inverse kinematics. 079 * 080 * @param chassisSpeeds The linear and angular (dx and dtheta) components that represent the 081 * chassis' speed. 082 * @return The left and right velocities. 083 */ 084 @Override 085 public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) { 086 return new DifferentialDriveWheelSpeeds( 087 chassisSpeeds.vx - trackwidth / 2 * chassisSpeeds.omega, 088 chassisSpeeds.vx + trackwidth / 2 * chassisSpeeds.omega); 089 } 090 091 @Override 092 public Twist2d toTwist2d( 093 DifferentialDriveWheelPositions start, DifferentialDriveWheelPositions end) { 094 return toTwist2d(end.left - start.left, end.right - start.right); 095 } 096 097 /** 098 * Performs forward kinematics to return the resulting Twist2d from the given left and right side 099 * distance deltas. This method is often used for odometry -- determining the robot's position on 100 * the field using changes in the distance driven by each wheel on the robot. 101 * 102 * @param leftDistance The distance measured by the left side encoder in meters. 103 * @param rightDistance The distance measured by the right side encoder in meters. 104 * @return The resulting Twist2d. 105 */ 106 public Twist2d toTwist2d(double leftDistance, double rightDistance) { 107 return new Twist2d( 108 (leftDistance + rightDistance) / 2, 0, (rightDistance - leftDistance) / trackwidth); 109 } 110 111 @Override 112 public DifferentialDriveWheelPositions copy(DifferentialDriveWheelPositions positions) { 113 return new DifferentialDriveWheelPositions(positions.left, positions.right); 114 } 115 116 @Override 117 public void copyInto( 118 DifferentialDriveWheelPositions positions, DifferentialDriveWheelPositions output) { 119 output.left = positions.left; 120 output.right = positions.right; 121 } 122 123 @Override 124 public DifferentialDriveWheelPositions interpolate( 125 DifferentialDriveWheelPositions startValue, 126 DifferentialDriveWheelPositions endValue, 127 double t) { 128 return startValue.interpolate(endValue, t); 129 } 130}