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 in meters per second. */ 018 public double frontLeft; 019 020 /** Speed of the front right wheel in meters per second. */ 021 public double frontRight; 022 023 /** Speed of the rear left wheel in meters per second. */ 024 public double rearLeft; 025 026 /** Speed of the rear right wheel in meters per second. */ 027 public double rearRight; 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 frontLeft Speed of the front left wheel in meters per second. 042 * @param frontRight Speed of the front right wheel in meters per second. 043 * @param rearLeft Speed of the rear left wheel in meters per second. 044 * @param rearRight Speed of the rear right wheel in meters per second. 045 */ 046 public MecanumDriveWheelSpeeds( 047 double frontLeft, double frontRight, double rearLeft, double rearRight) { 048 this.frontLeft = frontLeft; 049 this.frontRight = frontRight; 050 this.rearLeft = rearLeft; 051 this.rearRight = rearRight; 052 } 053 054 /** 055 * Constructs a MecanumDriveWheelSpeeds. 056 * 057 * @param frontLeft Speed of the front left wheel in meters per second. 058 * @param frontRight Speed of the front right wheel in meters per second. 059 * @param rearLeft Speed of the rear left wheel in meters per second. 060 * @param rearRight Speed of the rear right wheel in meters per second. 061 */ 062 public MecanumDriveWheelSpeeds( 063 LinearVelocity frontLeft, 064 LinearVelocity frontRight, 065 LinearVelocity rearLeft, 066 LinearVelocity rearRight) { 067 this( 068 frontLeft.in(MetersPerSecond), 069 frontRight.in(MetersPerSecond), 070 rearLeft.in(MetersPerSecond), 071 rearRight.in(MetersPerSecond)); 072 } 073 074 /** 075 * Renormalizes the wheel speeds if any individual speed 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 * @return Desaturated MecanumDriveWheelSpeeds. 084 */ 085 public MecanumDriveWheelSpeeds desaturate(double attainableMaxSpeed) { 086 double realMaxSpeed = Math.max(Math.abs(frontLeft), Math.abs(frontRight)); 087 realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearLeft)); 088 realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearRight)); 089 090 if (realMaxSpeed > attainableMaxSpeed) { 091 return new MecanumDriveWheelSpeeds( 092 frontLeft / realMaxSpeed * attainableMaxSpeed, 093 frontRight / realMaxSpeed * attainableMaxSpeed, 094 rearLeft / realMaxSpeed * attainableMaxSpeed, 095 rearRight / realMaxSpeed * attainableMaxSpeed); 096 } 097 098 return new MecanumDriveWheelSpeeds(frontLeft, frontRight, rearLeft, rearRight); 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 * @return Desaturated MecanumDriveWheelSpeeds. 111 */ 112 public MecanumDriveWheelSpeeds desaturate(LinearVelocity attainableMaxSpeed) { 113 return desaturate(attainableMaxSpeed.in(MetersPerSecond)); 114 } 115 116 /** 117 * Adds two MecanumDriveWheelSpeeds and returns the sum. 118 * 119 * <p>For example, MecanumDriveWheelSpeeds{1.0, 0.5, 2.0, 1.5} + MecanumDriveWheelSpeeds{2.0, 1.5, 120 * 0.5, 1.0} = MecanumDriveWheelSpeeds{3.0, 2.0, 2.5, 2.5} 121 * 122 * @param other The MecanumDriveWheelSpeeds to add. 123 * @return The sum of the MecanumDriveWheelSpeeds. 124 */ 125 public MecanumDriveWheelSpeeds plus(MecanumDriveWheelSpeeds other) { 126 return new MecanumDriveWheelSpeeds( 127 frontLeft + other.frontLeft, 128 frontRight + other.frontRight, 129 rearLeft + other.rearLeft, 130 rearRight + other.rearRight); 131 } 132 133 /** 134 * Subtracts the other MecanumDriveWheelSpeeds from the current MecanumDriveWheelSpeeds and 135 * returns the difference. 136 * 137 * <p>For example, MecanumDriveWheelSpeeds{5.0, 4.0, 6.0, 2.5} - MecanumDriveWheelSpeeds{1.0, 2.0, 138 * 3.0, 0.5} = MecanumDriveWheelSpeeds{4.0, 2.0, 3.0, 2.0} 139 * 140 * @param other The MecanumDriveWheelSpeeds to subtract. 141 * @return The difference between the two MecanumDriveWheelSpeeds. 142 */ 143 public MecanumDriveWheelSpeeds minus(MecanumDriveWheelSpeeds other) { 144 return new MecanumDriveWheelSpeeds( 145 frontLeft - other.frontLeft, 146 frontRight - other.frontRight, 147 rearLeft - other.rearLeft, 148 rearRight - other.rearRight); 149 } 150 151 /** 152 * Returns the inverse of the current MecanumDriveWheelSpeeds. This is equivalent to negating all 153 * components of the MecanumDriveWheelSpeeds. 154 * 155 * @return The inverse of the current MecanumDriveWheelSpeeds. 156 */ 157 public MecanumDriveWheelSpeeds unaryMinus() { 158 return new MecanumDriveWheelSpeeds(-frontLeft, -frontRight, -rearLeft, -rearRight); 159 } 160 161 /** 162 * Multiplies the MecanumDriveWheelSpeeds by a scalar and returns the new MecanumDriveWheelSpeeds. 163 * 164 * <p>For example, MecanumDriveWheelSpeeds{2.0, 2.5, 3.0, 3.5} * 2 = MecanumDriveWheelSpeeds{4.0, 165 * 5.0, 6.0, 7.0} 166 * 167 * @param scalar The scalar to multiply by. 168 * @return The scaled MecanumDriveWheelSpeeds. 169 */ 170 public MecanumDriveWheelSpeeds times(double scalar) { 171 return new MecanumDriveWheelSpeeds( 172 frontLeft * scalar, frontRight * scalar, rearLeft * scalar, rearRight * scalar); 173 } 174 175 /** 176 * Divides the MecanumDriveWheelSpeeds by a scalar and returns the new MecanumDriveWheelSpeeds. 177 * 178 * <p>For example, MecanumDriveWheelSpeeds{2.0, 2.5, 1.5, 1.0} / 2 = MecanumDriveWheelSpeeds{1.0, 179 * 1.25, 0.75, 0.5} 180 * 181 * @param scalar The scalar to divide by. 182 * @return The scaled MecanumDriveWheelSpeeds. 183 */ 184 public MecanumDriveWheelSpeeds div(double scalar) { 185 return new MecanumDriveWheelSpeeds( 186 frontLeft / scalar, frontRight / scalar, rearLeft / scalar, rearRight / scalar); 187 } 188 189 @Override 190 public String toString() { 191 return String.format( 192 "MecanumDriveWheelSpeeds(Front Left: %.2f m/s, Front Right: %.2f m/s, " 193 + "Rear Left: %.2f m/s, Rear Right: %.2f m/s)", 194 frontLeft, frontRight, rearLeft, rearRight); 195 } 196}