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.MetersPerSecond; 008 009import edu.wpi.first.math.kinematics.proto.DifferentialDriveWheelSpeedsProto; 010import edu.wpi.first.math.kinematics.struct.DifferentialDriveWheelSpeedsStruct; 011import edu.wpi.first.units.measure.LinearVelocity; 012import edu.wpi.first.util.protobuf.ProtobufSerializable; 013import edu.wpi.first.util.struct.StructSerializable; 014 015/** Represents the wheel speeds for a differential drive drivetrain. */ 016public class DifferentialDriveWheelSpeeds implements ProtobufSerializable, StructSerializable { 017 /** Speed of the left side of the robot in meters per second. */ 018 public double left; 019 020 /** Speed of the right side of the robot in meters per second. */ 021 public double right; 022 023 /** DifferentialDriveWheelSpeeds protobuf for serialization. */ 024 public static final DifferentialDriveWheelSpeedsProto proto = 025 new DifferentialDriveWheelSpeedsProto(); 026 027 /** DifferentialDriveWheelSpeeds struct for serialization. */ 028 public static final DifferentialDriveWheelSpeedsStruct struct = 029 new DifferentialDriveWheelSpeedsStruct(); 030 031 /** Constructs a DifferentialDriveWheelSpeeds with zeros for left and right speeds. */ 032 public DifferentialDriveWheelSpeeds() {} 033 034 /** 035 * Constructs a DifferentialDriveWheelSpeeds. 036 * 037 * @param left The left speed in meters per second. 038 * @param right The right speed in meters per second. 039 */ 040 public DifferentialDriveWheelSpeeds(double left, double right) { 041 this.left = left; 042 this.right = right; 043 } 044 045 /** 046 * Constructs a DifferentialDriveWheelSpeeds. 047 * 048 * @param left The left speed in meters per second. 049 * @param right The right speed in meters per second. 050 */ 051 public DifferentialDriveWheelSpeeds(LinearVelocity left, LinearVelocity right) { 052 this(left.in(MetersPerSecond), right.in(MetersPerSecond)); 053 } 054 055 /** 056 * Renormalizes the wheel speeds if any either side is above the specified maximum. 057 * 058 * <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be 059 * above the max attainable speed for the driving motor on that wheel. To fix this issue, one can 060 * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the 061 * absolute threshold, while maintaining the ratio of speeds between wheels. 062 * 063 * @param attainableMaxSpeed The absolute max speed in meters per second that a wheel can reach. 064 */ 065 public void desaturate(double attainableMaxSpeed) { 066 double realMaxSpeed = Math.max(Math.abs(left), Math.abs(right)); 067 068 if (realMaxSpeed > attainableMaxSpeed) { 069 left = left / realMaxSpeed * attainableMaxSpeed; 070 right = right / realMaxSpeed * attainableMaxSpeed; 071 } 072 } 073 074 /** 075 * Renormalizes the wheel speeds if any either side is above the specified maximum. 076 * 077 * <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be 078 * above the max attainable speed for the driving motor on that wheel. To fix this issue, one can 079 * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the 080 * absolute threshold, while maintaining the ratio of speeds between wheels. 081 * 082 * @param attainableMaxSpeed The absolute max speed in meters per second that a wheel can reach. 083 */ 084 public void desaturate(LinearVelocity attainableMaxSpeed) { 085 desaturate(attainableMaxSpeed.in(MetersPerSecond)); 086 } 087 088 /** 089 * Adds two DifferentialDriveWheelSpeeds and returns the sum. 090 * 091 * <p>For example, DifferentialDriveWheelSpeeds{1.0, 0.5} + DifferentialDriveWheelSpeeds{2.0, 1.5} 092 * = DifferentialDriveWheelSpeeds{3.0, 2.0} 093 * 094 * @param other The DifferentialDriveWheelSpeeds to add. 095 * @return The sum of the DifferentialDriveWheelSpeeds. 096 */ 097 public DifferentialDriveWheelSpeeds plus(DifferentialDriveWheelSpeeds other) { 098 return new DifferentialDriveWheelSpeeds(left + other.left, right + other.right); 099 } 100 101 /** 102 * Subtracts the other DifferentialDriveWheelSpeeds from the current DifferentialDriveWheelSpeeds 103 * and returns the difference. 104 * 105 * <p>For example, DifferentialDriveWheelSpeeds{5.0, 4.0} - DifferentialDriveWheelSpeeds{1.0, 2.0} 106 * = DifferentialDriveWheelSpeeds{4.0, 2.0} 107 * 108 * @param other The DifferentialDriveWheelSpeeds to subtract. 109 * @return The difference between the two DifferentialDriveWheelSpeeds. 110 */ 111 public DifferentialDriveWheelSpeeds minus(DifferentialDriveWheelSpeeds other) { 112 return new DifferentialDriveWheelSpeeds(left - other.left, right - other.right); 113 } 114 115 /** 116 * Returns the inverse of the current DifferentialDriveWheelSpeeds. This is equivalent to negating 117 * all components of the DifferentialDriveWheelSpeeds. 118 * 119 * @return The inverse of the current DifferentialDriveWheelSpeeds. 120 */ 121 public DifferentialDriveWheelSpeeds unaryMinus() { 122 return new DifferentialDriveWheelSpeeds(-left, -right); 123 } 124 125 /** 126 * Multiplies the DifferentialDriveWheelSpeeds by a scalar and returns the new 127 * DifferentialDriveWheelSpeeds. 128 * 129 * <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} * 2 = DifferentialDriveWheelSpeeds{4.0, 130 * 5.0} 131 * 132 * @param scalar The scalar to multiply by. 133 * @return The scaled DifferentialDriveWheelSpeeds. 134 */ 135 public DifferentialDriveWheelSpeeds times(double scalar) { 136 return new DifferentialDriveWheelSpeeds(left * scalar, right * scalar); 137 } 138 139 /** 140 * Divides the DifferentialDriveWheelSpeeds by a scalar and returns the new 141 * DifferentialDriveWheelSpeeds. 142 * 143 * <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} / 2 = DifferentialDriveWheelSpeeds{1.0, 144 * 1.25} 145 * 146 * @param scalar The scalar to divide by. 147 * @return The scaled DifferentialDriveWheelSpeeds. 148 */ 149 public DifferentialDriveWheelSpeeds div(double scalar) { 150 return new DifferentialDriveWheelSpeeds(left / scalar, right / scalar); 151 } 152 153 @Override 154 public String toString() { 155 return String.format( 156 "DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)", left, right); 157 } 158}