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