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.MecanumDriveWheelSpeedsProto; 010import edu.wpi.first.math.kinematics.struct.MecanumDriveWheelSpeedsStruct; 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 mecanum drive drivetrain. */ 016public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSerializable { 017 /** Speed of the front left wheel. */ 018 public double frontLeftMetersPerSecond; 019 020 /** Speed of the front right wheel. */ 021 public double frontRightMetersPerSecond; 022 023 /** Speed of the rear left wheel. */ 024 public double rearLeftMetersPerSecond; 025 026 /** Speed of the rear right wheel. */ 027 public double rearRightMetersPerSecond; 028 029 /** MecanumDriveWheelSpeeds protobuf for serialization. */ 030 public static final MecanumDriveWheelSpeedsProto proto = new MecanumDriveWheelSpeedsProto(); 031 032 /** MecanumDriveWheelSpeeds struct for serialization. */ 033 public static final MecanumDriveWheelSpeedsStruct struct = new MecanumDriveWheelSpeedsStruct(); 034 035 /** Constructs a MecanumDriveWheelSpeeds with zeros for all member fields. */ 036 public MecanumDriveWheelSpeeds() {} 037 038 /** 039 * Constructs a MecanumDriveWheelSpeeds. 040 * 041 * @param frontLeftMetersPerSecond Speed of the front left wheel. 042 * @param frontRightMetersPerSecond Speed of the front right wheel. 043 * @param rearLeftMetersPerSecond Speed of the rear left wheel. 044 * @param rearRightMetersPerSecond Speed of the rear right wheel. 045 */ 046 public MecanumDriveWheelSpeeds( 047 double frontLeftMetersPerSecond, 048 double frontRightMetersPerSecond, 049 double rearLeftMetersPerSecond, 050 double rearRightMetersPerSecond) { 051 this.frontLeftMetersPerSecond = frontLeftMetersPerSecond; 052 this.frontRightMetersPerSecond = frontRightMetersPerSecond; 053 this.rearLeftMetersPerSecond = rearLeftMetersPerSecond; 054 this.rearRightMetersPerSecond = rearRightMetersPerSecond; 055 } 056 057 /** 058 * Constructs a MecanumDriveWheelSpeeds. 059 * 060 * @param frontLeft Speed of the front left wheel. 061 * @param frontRight Speed of the front right wheel. 062 * @param rearLeft Speed of the rear left wheel. 063 * @param rearRight Speed of the rear right wheel. 064 */ 065 public MecanumDriveWheelSpeeds( 066 LinearVelocity frontLeft, 067 LinearVelocity frontRight, 068 LinearVelocity rearLeft, 069 LinearVelocity rearRight) { 070 this( 071 frontLeft.in(MetersPerSecond), 072 frontRight.in(MetersPerSecond), 073 rearLeft.in(MetersPerSecond), 074 rearRight.in(MetersPerSecond)); 075 } 076 077 /** 078 * Renormalizes the wheel speeds if any individual speed is above the specified maximum. 079 * 080 * <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be 081 * above the max attainable speed for the driving motor on that wheel. To fix this issue, one can 082 * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the 083 * absolute threshold, while maintaining the ratio of speeds between wheels. 084 * 085 * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach. 086 */ 087 public void desaturate(double attainableMaxSpeedMetersPerSecond) { 088 double realMaxSpeed = 089 Math.max(Math.abs(frontLeftMetersPerSecond), Math.abs(frontRightMetersPerSecond)); 090 realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearLeftMetersPerSecond)); 091 realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearRightMetersPerSecond)); 092 093 if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) { 094 frontLeftMetersPerSecond = 095 frontLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond; 096 frontRightMetersPerSecond = 097 frontRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond; 098 rearLeftMetersPerSecond = 099 rearLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond; 100 rearRightMetersPerSecond = 101 rearRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond; 102 } 103 } 104 105 /** 106 * Renormalizes the wheel speeds if any individual speed is above the specified maximum. 107 * 108 * <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be 109 * above the max attainable speed for the driving motor on that wheel. To fix this issue, one can 110 * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the 111 * absolute threshold, while maintaining the ratio of speeds between wheels. 112 * 113 * @param attainableMaxSpeed The absolute max speed that a wheel can reach. 114 */ 115 public void desaturate(LinearVelocity attainableMaxSpeed) { 116 desaturate(attainableMaxSpeed.in(MetersPerSecond)); 117 } 118 119 /** 120 * Adds two MecanumDriveWheelSpeeds and returns the sum. 121 * 122 * <p>For example, MecanumDriveWheelSpeeds{1.0, 0.5, 2.0, 1.5} + MecanumDriveWheelSpeeds{2.0, 1.5, 123 * 0.5, 1.0} = MecanumDriveWheelSpeeds{3.0, 2.0, 2.5, 2.5} 124 * 125 * @param other The MecanumDriveWheelSpeeds to add. 126 * @return The sum of the MecanumDriveWheelSpeeds. 127 */ 128 public MecanumDriveWheelSpeeds plus(MecanumDriveWheelSpeeds other) { 129 return new MecanumDriveWheelSpeeds( 130 frontLeftMetersPerSecond + other.frontLeftMetersPerSecond, 131 frontRightMetersPerSecond + other.frontRightMetersPerSecond, 132 rearLeftMetersPerSecond + other.rearLeftMetersPerSecond, 133 rearRightMetersPerSecond + other.rearRightMetersPerSecond); 134 } 135 136 /** 137 * Subtracts the other MecanumDriveWheelSpeeds from the current MecanumDriveWheelSpeeds and 138 * returns the difference. 139 * 140 * <p>For example, MecanumDriveWheelSpeeds{5.0, 4.0, 6.0, 2.5} - MecanumDriveWheelSpeeds{1.0, 2.0, 141 * 3.0, 0.5} = MecanumDriveWheelSpeeds{4.0, 2.0, 3.0, 2.0} 142 * 143 * @param other The MecanumDriveWheelSpeeds to subtract. 144 * @return The difference between the two MecanumDriveWheelSpeeds. 145 */ 146 public MecanumDriveWheelSpeeds minus(MecanumDriveWheelSpeeds other) { 147 return new MecanumDriveWheelSpeeds( 148 frontLeftMetersPerSecond - other.frontLeftMetersPerSecond, 149 frontRightMetersPerSecond - other.frontRightMetersPerSecond, 150 rearLeftMetersPerSecond - other.rearLeftMetersPerSecond, 151 rearRightMetersPerSecond - other.rearRightMetersPerSecond); 152 } 153 154 /** 155 * Returns the inverse of the current MecanumDriveWheelSpeeds. This is equivalent to negating all 156 * components of the MecanumDriveWheelSpeeds. 157 * 158 * @return The inverse of the current MecanumDriveWheelSpeeds. 159 */ 160 public MecanumDriveWheelSpeeds unaryMinus() { 161 return new MecanumDriveWheelSpeeds( 162 -frontLeftMetersPerSecond, 163 -frontRightMetersPerSecond, 164 -rearLeftMetersPerSecond, 165 -rearRightMetersPerSecond); 166 } 167 168 /** 169 * Multiplies the MecanumDriveWheelSpeeds by a scalar and returns the new MecanumDriveWheelSpeeds. 170 * 171 * <p>For example, MecanumDriveWheelSpeeds{2.0, 2.5, 3.0, 3.5} * 2 = MecanumDriveWheelSpeeds{4.0, 172 * 5.0, 6.0, 7.0} 173 * 174 * @param scalar The scalar to multiply by. 175 * @return The scaled MecanumDriveWheelSpeeds. 176 */ 177 public MecanumDriveWheelSpeeds times(double scalar) { 178 return new MecanumDriveWheelSpeeds( 179 frontLeftMetersPerSecond * scalar, 180 frontRightMetersPerSecond * scalar, 181 rearLeftMetersPerSecond * scalar, 182 rearRightMetersPerSecond * scalar); 183 } 184 185 /** 186 * Divides the MecanumDriveWheelSpeeds by a scalar and returns the new MecanumDriveWheelSpeeds. 187 * 188 * <p>For example, MecanumDriveWheelSpeeds{2.0, 2.5, 1.5, 1.0} / 2 = MecanumDriveWheelSpeeds{1.0, 189 * 1.25, 0.75, 0.5} 190 * 191 * @param scalar The scalar to divide by. 192 * @return The scaled MecanumDriveWheelSpeeds. 193 */ 194 public MecanumDriveWheelSpeeds div(double scalar) { 195 return new MecanumDriveWheelSpeeds( 196 frontLeftMetersPerSecond / scalar, 197 frontRightMetersPerSecond / scalar, 198 rearLeftMetersPerSecond / scalar, 199 rearRightMetersPerSecond / scalar); 200 } 201 202 @Override 203 public String toString() { 204 return String.format( 205 "MecanumDriveWheelSpeeds(Front Left: %.2f m/s, Front Right: %.2f m/s, " 206 + "Rear Left: %.2f m/s, Rear Right: %.2f m/s)", 207 frontLeftMetersPerSecond, 208 frontRightMetersPerSecond, 209 rearLeftMetersPerSecond, 210 rearRightMetersPerSecond); 211 } 212}